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ABSTRACT 


Title  of  Dissertation:  MOTION  CONTROL  AND  PLANNING 

FOR  NONHOLONOMIC  KINEMATIC  CHAINS 

Dimitrios  P.  Tsakiris,  Doctor  of  Philosophy,  1995 

Dissertation  directed  by:  Professor  P.  S.  Krishnaprasad 

Department  of  Electrical  Engineering 


In  this  dissertation  we  examine  a  class  of  systems  where  nonholonomic  kinematic 
constraints  are  combined  with  periodic  shape  variations,  giving  rise  to  a  snake-like  undu¬ 
lating  motion  of  the  system.  Within  this  class,  we  distinguish  two  subclasses,  one  where 
the  system  possesses  enough  kinematic  constraints  to  allow  the  control  of  its  motion 
to  be  based  entirely  on  kinematics  and  another  which  does  not;  in  the  latter  case,  the 
dynamics  plays  a  crucial  role  in  complementing  the  kinematics  and  in  making  motion 
control  possible.  An  instance  of  these  systems  are  the  Nonholonomic  Variable  Geometry 
Truss  (NVGT)  assemblies,  where  shape  changes  are  implemented  by  parallel  manipula¬ 
tor  modules,  while  the  nonholonomic  constraints  are  imposed  by  idler  wheels  attached 
to  the  assembly.  We  assume  that  the  wheels  roll  without  slipping  on  the  ground,  thus 
constraining  the  instantaneous  motion  of  the  assembly.  These  assemblies  can  be  consid¬ 
ered  as  land  locomotion  alternatives  to  systems  based  on  legs  or  actuated  wheels.  Their 
propulsion  combines  features  of  both  biological  systems  like  skating  humans  and  snakes, 
and  of  man-made  systems  like  orbiting  satellites  with  manipulator  arms.  The  NVGT 
assemblies  can  be  modeled  in  terms  of  the  Special  Euclidean  group  of  rigid  motions  on 
the  plane.  Generalization  to  nonholonomic  kinematic  chains  on  other  Lie  groups  ( G ) 
gives  rise  to  the  notion  of  G-Snakes. 

Moreover,  we  examine  systems  with  parallel  manipulator  subsystems  which  can  be 
used  as  sensor-carrying  platforms,  with  potential  applications  in  exploratory  and  active 
visual  or  haptic  robotic  tasks.  We  concentrate  on  specifying  a  class  of  configuration  space 
path  segments  that  are  optimal  in  the  sense  of  a  curvature-squared  cost  functional,  which 


can  be  specified  analytically  in  terms  of  elliptic  functions  and  can  be  used  to  synthesize 
a  trajectory  of  the  system. 

In  both  cases,  a  setup  of  the  problem  which  involves  tools  from  differential  geometry 
and  the  theory  of  Lie  groups  appears  to  be  natural.  In  the  case  of  G-Snakes,  when  the 
number  of  nonholonomic  constraints  equals  the  dimension  of  the  group  G,  the  constraints 
determine  a  principal  fiber  bundle  connection.  The  geometric  phase  associated  to  this 
connection  allows  us  to  derive  (kinematic)  motion  control  strategies  based  on  periodic 
shape  variations  of  the  system.  When  the  G-Snake  assembly  has  one  constraint  less 
than  the  dimension  of  the  group  G,  we  are  still  able  to  synthesize  a  principal  fiber  bundle 
connection  by  taking  into  account  the  Lagrangian  dynamics  of  the  system  through  the  so- 
called  nonholonomic  momentum.  The  symmetries  of  the  system  are  captured  by  actions 
of  non-abelian  Lie  groups  that  leave  invariant  both  the  constraints  and  the  Lagrangian 
and  play  a  significant  role  in  the  definition  of  the  momentum  and  the  specification  of  its 
evolution.  The  (dynamic)  motion  control  is  now  based  on  periodic  shape  variations  that 
build  up  momentum  and  allow  propulsion  and  steering,  as  described  by  the  geometric 
and  dynamic  phases  of  the  system. 
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CHAPTER  ONE 


PROLEGOMENA 


One  of  the  main  topics  of  this  dissertation  is  the  study  of  a  novel  class  of  robotic  de¬ 
vices  that  combine  features  of  such  biological  systems  as  snakes  and  skating  humans  and 
of  such  man-made  systems  as  orbiting  satellites  equipped  with  manipulator  arms.  Those 
multimodule  devices,  which  we  call  Nonholonomic  Variable  Geometry  Truss  (NVGT)  as¬ 
semblies  and  which  can  be  used  for  locomotion  as  an  alternative  to  cars  or  bicycles,  are 
equipped  with  wheels,  which  we  assume  that  they  roll  without  slipping  on  the  plane 
that  supports  the  device.  Like  the  front  wheel  of  a  bicycle,  those  wheels  are  not  directly 
actuated;  they  merely  constrain  the  instantaneous  motion  of  the  assembly  so  that  it 
does  not  slip  “sideways” .  This  type  of  constraints  belong  to  the  class  of  so-called  non¬ 
holonomic  constraints,  instances  of  which  also  appear  in  the  systems  mentioned  earlier. 

I 

Their  features  are  presented  in  more  detail  later.  Each  module  of  the  assembly  can  alter 
its  shape  in  a  periodic  way  by  a  suitable  mechanism  of  the  Variable  Geometry  Truss 
(VGT)  variety,  implemented  as  a  planar  parallel  manipulator. 

Usual  land  locomotion  systems  employ  either  legs  (humans,  insects)  or  an  endless 
rotating  element,  like  actuated  wheels  or  tracks,  for  propulsion.  An  alternative,  that  is 
currently  emerging  in  robotic  studies,  employs  articulated  bodies  and  suitable  motion 
constraints  in  a  paradigm  inspired  by  snake  and  worm  locomotion.  The  locomotion 
of  the  NVGT  is  achieved  by  the  interaction  of  the  nonholonomic  constraints  from  the 
wheels,  with  the  periodic  shape  variations  of  the  modules.  This  gives  rise  to  a  snake-like 
undulating  motion  of  the  assembly.  The  kinematic  and  dynamic  analysis  of  such  systems 
unveils  an  interesting  geometric  structure  and  makes  explicit  their  fundamental  property 
of  specifying  the  global  motion  of  the  system  as  a  function  of  just  its  shape  and  of  the 
shape  variations.  This  property  is  related  to  the  phenomenon  of  geometric  phases  that 
appear  in  several  physical  systems  and  it  can  be  exploited  for  planning  and  controlling 
their  motion;  however  it’s  not  obvious  how  to  solve  those  problems,  since  the  NVGT 
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systems  are  very  redundant  and  the  proper  selection  of  the  shape  actuation  strategy 
that  will  achieve  a  desired  motion  of  the  system  is  made  even  more  complicated  by  the 
presence  of  the  nonholonomic  constraints. 

This  dissertation  also  examines  robotic  systems  with  parallel  manipulator  subsys¬ 
tems  which  are  used  as  sensor-carrying  platforms  to  perform  exploratory  visual  or  haptic 
tasks.  Exploratory  and  active  tasks  occur  when  a  robotic  system  relocates  its  mechanical 
and/or  sensory  subsystems  or  alters  their  characteristics  in  order  to  collect  information 
about  its  environment,  about  the  tasks  it  is  required  to  perform  and,  possibly,  about  the 
robotic  system  itself.  Actions  of  this  type  can  also  facilitate  the  subsequent  processing 
and  understanding  by  the  system  of  the  sensory  information  that  was  obtained,  since 
both  spatially  and  temporally  novel  information  is  added  in  a  controlled  way.  This  is  a 
relatively  novel  paradigm  in  robotics  that  is  having  a  profound  impact  in  several  related 
research  areas  (Negahdaripour  &  Jain  [1991]).  Vision  is  one  of  the  sensory  modalities 
where  the  active  approach  has  been  well  documented.  This  was  inspired  by  the  obser¬ 
vation  that  humans  tend  to  move  their  eyes  and  heads  in  a  variety  of  ways,  in  order  to 
obtain  a  better  view  of  a  scene  or  in  order  to  focus  on  a  moving  object  of  interest,  all  of 
those  activities  demonstrating  that  human  perception  of  a  dynamically  evolving  scene  is 
an  equally  dynamical  act,  whether  conscious  or  unconscious  (Bajcsy  [1988]).  This  has 
found  robotic  applications,  as  in  the  case  where  a  camera  is  being  moved  around  the 
workspace  of  the  robot  for  obtaining  multiple  views  of  it  (Zheng,  Chen  &  Tsuji  [1991]), 
for  monocular  stereo-vision  (Sandini  &  Tistarelli  [1990])  or  for  facilitating  dynamic  seg¬ 
mentation  (Aloimonos  [1990]).  Furthermore,  the  implementation  of  saccadic,  smooth 
pursuit  or  vergence  camera  motions,  as  well  as  the  repositioning  of  cameras  are  well 
known  methods  for  moving  target  stabilization  (Papanikolopoulos,  Khosla  &  Kanade 
[1991]),  for  camera  fixation  and  gaze  control  (Brown  [1990];  Raviv  [1991]),  for  easy 
egomotion  parameter  computation,  for  conversion  of  ill-posed  early  vision  problems  into 
well-posed  ones  (Aloimonos,  Weiss  &  Bandopadhay  [1988])  and  for  changing  the  focus  of 
attention  of  a  robotic  system  (Abbott  [1992];  Clark  &  Ferrier  [1989]).  In  previous  work, 
we  examined  visual  target  tracking  based  on  the  use  of  sequences  of  images  that  specify 
the  camera  reorientation  needed  so  that  a  target  moving  in  a  cluttered  3-dimensional 
visual  environment  is  kept  foveated  (Aloimonos  &  Tsakiris  [1991]).  In  order  to  bypass 
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the  image  segmentation  and  feature  correspondence  problems  inherent  in  other  tracking 
methods,  as  well  as  the  restriction  to  2-dimensional  domains,  we  employed  the  optical 
flow  formalism.  This  however  necessitates  the  use  of  a  dense  image  sequence  as  an  input 
to  the  tracking  algorithm,  which  in  turn  implies  that  small  and  accurate  motions  of  the 
camera  are  necessary.  Haptic  exploratory  tasks  are  another  example  of  the  active  ap¬ 
proach.  They  can  use  tactile  sensors  mounted  on  fingertips  of  dextrous  hands  with  the 
goal  to  derive  information  on  the  shape,  the  surface  texture  and  the  mechanical  prop¬ 
erties  of  objects  of  interest.  This  may  involve  suitable  accurate  motion  of  the  sensors, 
while  contact  with  those  objects  is  maintained  (Bajcsy,  Lederman  &  Klatzky  [1989]; 
Dario  [1989];  Ellis  [1990];  Loncaric  et  al.  [1989]). 

Robotic  systems  therefore,  may  include  a  mechanism  that  carries  the  sensors,  has 
the  ability  to  translate  or  reorient  them  and  is  able  to  perform  fine,  accurate  and,  at 
the  same  time,  fast  motions.  Previous  designs  use  either  conventional  robots  or  pan- 
and-tilt  platforms  as  a  mechanism  for  carrying  the  sensors  (Ballard  &  Ozcandarli  [1988]; 
Clark  &  Ferrier  [1988];  Krotkov  [1989]).  But  conventional  serial  manipulators  generally 
lack  the  dynamic  response  characteristics  to  satisfy  the  above  requirements  on  the  speed 
and  accuracy  of  motion,  while  pan-and-tilt  platforms  have  limited  degrees-of-freedom. 
An  alternative  is  to  implement  this  sensor-carrying  platform  as  a  parallel  manipulator 
(Tsakiris  &  Aloimonos  [1989];  Tsakiris  &  Krishnaprasad  [1993]).  An  example  of  such  a 
system  is  the  so-called  “Stewart  platform” ,  where  six  legs  with  linear  motors  support  a 
platform  carrying  the  sensors.  By  varying  the  length  of  the  legs,  the  platform  translates 
and/or  reorients  itself.  This  platform  can  be  attached  at  the  end-effector  of  a  serial 
robot  and  provides  a  light-weight,  yet  strong  and  accurate  system  with  full  6  degrees-of- 
freedom  motion.  There  exists  a  vast  literature  on  the  mechanical  design,  the  kinematics, 
the  dynamics  and  the  control  of  parallel  manipulators  (Cleary  &  Arai  [1991];  Do  &  Yang 
[1988];  Fichter  [1986];  Gosselin  &  Angeles  [1988];  Hudgens  &;  Tesar  [1988];  Hunt  [1983]; 
Merlet  [1987];  Nanua,  Waldron  &  Murthy  [1990];  Pfreundschuh,  Kumar  &  Sugar  [1991]; 
Stewart  [1966];  Sugimoto  [1989];  Tsai  &  Tahmasebi  [1991]). 

In  Chapter  2,  we  collect  some  tools  from  Differential  Geometry,  Geometric  Mechan¬ 
ics  and  the  theory  of  Elliptic  functions,  which  will  be  needed  in  the  sequel.  We  also 
review  the  kinematics  of  parallel  manipulators. 
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In  Chapter  3  we  focus  on  the  motion  planning  problem  for  parallel  manipulators, 
which  is  a  problem  that  has  not  received  as  much  attention  as  the  problems  mentioned 
earlier.  We  will  attempt  to  describe  the  geometry  of  the  manifold  of  singular  configura¬ 
tions  on  the  configuration  space  for  a  particular  design  of  parallel  manipulator,  which  is 
generic  enough  to  exhibit  interesting  kinematic  properties,  but  simple  enough  to  allow 
our  results  not  to  be  overshadowed  by  complex  calculations  and  use  the  results  of  motion 
planning  for  avoidance  of  those  singular  configurations. 

Planning  sequences  of  actions  is  one  of  the  earliest  areas  of  research  in  Artificial 
Intelligence,  but  most  of  this  early  work  has  only  considered  idealized  domains,  where  a 
robot  can  be  exactly  controlled  and  where  task-level  commands  are  sufficient  to  describe 
the  behavior  of  the  system  (Lozano-Perez  [1987];  Nilsson  [1980]).  In  robotic  applications 
there  is  a  need  of  planning  the  motions  of  robots  in  practical  domains,  where  obstacles, 
uncertainties,  errors  and  various  constraints  are  present  (Brady  [1989]).  In  this  spirit,  the 
problem  of  planning  collision-free  motions  in  the  workspace  of  a  robot  has  been  consid¬ 
ered  more  recently.  Furthermore,  the  problem  of  planning  motions  in  cases  where  certain 
holonomic  or  non-  holonomic  constraints  apply,  or  of  motions  where  certain  performance 
criteria  based  on  kinematic  or  dynamic  characteristics  of  the  motion  are  optimized,  are 
of  significant  interest  (Latombe  [1991]). 

In  the  case  of  serial  manipulators,  planning  in  “joint  space”  is  considered  computa¬ 
tionally  more  efficient  compared  to  “Cartesian  space”  planning,  because  of  the  need,  in 
the  later  case,  to  solve  the  inverse  kinematics  problem  at  run  time  (Craig  [1986]).  This 
makes  a  desired  Cartesian  space  trajectory  achievable  only  approximately  (Brady  et  al. 
[1983]).  However,  for  parallel  manipulators  this  need  not  be  the  case,  since  the  inverse 
kinematics  is  easily  and  uniquely  solvable,  thus  planning  in  Cartesian  space  becomes 
feasible.  Since  the  orientation  of  the  platform  is  also  of  significant  interest,  especially 
when  the  platform  carries  vision  or  tactile  sensors,  we  consider  planning  in  the  config¬ 
uration  space  of  the  parallel  manipulator.  Moreover,  sensory  information  interpretation 
subsystems  typically  use  configuration  space-based  reasoning  and  this  has  to  be  taken 
into  account  in  planning  the  motions  of  a  robotic  system. 

Our  main  problem  is  the  specification  of  a  trajectory  for  the  parallel  manipulator  sys¬ 
tem  that  has  optimal  shape  characteristics  and  meets  prespecified  boundary  conditions. 
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Such  a  trajectory  is  a  curve  in  the  configuration  space,  which  is  usually  a  subgroup  of  the 
Special  Euclidean  group  SE( 2).  We  examine  curves  on  those  spaces,  which  are  optimal 
in  the  sense  of  a  curvature-square  cost  functional.  Differential  Geometric  tools  are  used 
to  transform  this  problem  into  a  nonlinear  optimal  control  problem  on  a  left-invariant 
dynamical  system  and  this  is  solved  using  a  suitable  generalization  of  the  Maximum 
Principle.  In  the  case  of  motions  of  planar  parallel  manipulators  where  the  configuration 
space  is  a  2-dimensional  manifold,  we  characterize  the  curvature  of  an  optimal  curve  as 
the  solution  of  a  differential  equation  of  the  Bryant-Griffiths  type,  which,  in  the  generic 
case,  is  given  by  elliptic  functions.  Parallel  manipulators  demonstrate  an  indeterminacy 
in  the  specification  of  their  future  behavior  while  they  are  at  singular  configurations, 
as  well  as  an  inability  to  resist  forces  and  torques  in  specific  directions,  since,  contrary 
to  serial  manipulators,  they  gain  degrees-of-freedom  in  those  configurations.  Those  are 
undesirable  characteristics  for  our  robotic  system  and  we  try,  either  to  avoid  kinematic 
singularities  as  much  as  possible  by  proper  mechanical  design  and  careful  planning  of 
the  system  trajectories,  or  to  design  our  crossing  of  the  singular  surfaces  in  a  specific 
way  that  will  allow  the  dynamics  of  our  system  to  resolve  those  indeterminacies.  To  this 
end,  we  map  explicitly  the  singular  surfaces  of  the  parallel  manipulators  that  we  use  and 
we  employ  the  family  of  optimal  curves  derived  from  motion  planning  to  optimize  the 
singular  surface  crossing  characteristics  of  the  system  trajectories.  Extensions  of  this 
motion  planning  technique  to  the  general  planar  case,  where  the  configuration  space  is 
a  3-dimensional  manifold  are  also  discussed. 

The  problems  in  chapter  3  focused  on  systems  with  holonomic  constraints,  i.e.  al¬ 
gebraic  constraints  involving  the  configuration  variables  of  the  system.  The  class  of 
nonholonomic  constraints ,  where  the  constraints  involve  the  velocities  of  the  system, 
but  cannot  be  integrated  to  produce  holonomic  constraints,  is  currently  receiving  atten¬ 
tion  because  of  its  importance  in  manipulation,  mobile  robots  and  locomotion  problems 
(Latombe  [1991];  Murray,  Li  &  Sastry  [1994]).  A  typical  example  is  a  car  with  wheels 
that  roll  without  slipping  on  the  ground.  The  motion  of  the  car  is  restricted  instanta¬ 
neously,  since  it  cannot  slip  sideways,  however  this  does  not  restrict  its  position,  since 
by  the  well-known  parallel  parking  maneuver,  the  car  can  effectively  move  sideways  over 
a  finite  time  period.  The  main  tool  for  checking  whether  a  set  of  constraints  (usually 
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expressed  as  one-forms)  is  integrable,  is  Frobenius’  Theorem,  which  states  that  for  this 
to  happen,  the  distribution  which  annihilates  the  constraints  should  be  involutive,  i.e. 
closed  under  Lie  bracketing.  In  the  remainder  of  this  work  we  will  consider  systems 
where  this  is  not  true. 

In  Chapters  4,  5  and  6,  we  focus  on  mechanical  systems  subject  to  nonholonomic 
constraints,  where  variations  of  shape  induce,  under  the  influence  of  the  constraints,  a 
global  motion  of  the  system.  A  well-known  example  of  such  systems  is  a  free-floating 
multibody  system  in  space  (e.g.  robotic  manipulators  mounted  on  orbiting  satellites), 
where  periodic  movements  of  the  joints  induce  a  reorientation  of  the  system,  under  the 
nonholonomic  constraint  of  conservation  of  angular  momentum  (Krishnaprasad  [1990]; 
Marsden,  Montgomery  &  Ratiu  [1990]). 

In  Chapter  5,  we  describe  in  some  detail  a  novel  system,  introduced  in  (Krish¬ 
naprasad  &  Tsakiris  [1993];  Krishnaprasad  &  Tsakiris  [1994b]),  which  uses  the  above 
principle  for  land  locomotion.  This  was  inspired  by  the  experimental  work  of  Joel  Bur¬ 
dick  and  his  students  at  Caltech  (Chirikjian  &  Burdick  [1991];  Chirikjian  &  Burdick 
[1993]).  This  system,  called  the  Nonholonomic  Variable  Geometry  Truss  (NVGT)  as¬ 
sembly,  consists  of  longitudinal  repetition  of  truss  modules,  each  one  of  which  is  equipped 
with  idler  wheels  and  linear  actuators  in  a  planar  parallel  manipulator  configuration,  and 
uses  periodic  changes  of  the  shape  of  each  module  to  propel  itself.  The  locomotion  princi¬ 
ple  is  not  based  on  direct  actuation  of  wheels,  but  rather  on  the  nonholonomic  constraints 
imposed  on  the  motion  of  the  system  by  the  rolling  without  slipping  of  the  idler  wheels  of 
each  module  on  the  supporting  plane.  This  results  in  a  snake-like  motion  of  the  NVGT 
assembly,  which  is  not  too  far,  at  least  in  principle,  from  certain  modes  of  actual  snake 
locomotion  (Hirose  [1993]).  Both  the  shape  and  the  configuration  of  the  NVGT  assembly 
can  be  described  by  elements  of  the  Special  Euclidean  group  SE( 2),  the  group  of  rigid 
motions  on  the  plane.  A  system  like  the  NVGT  assembly  constitutes  a  kinematic  chain 
evolving  on  this  matrix  Lie  group  G ,  with  the  corresponding  velocities  given  by  elements 
of  the  Lie  algebra  Q  of  G.  Of  these  velocities,  the  shape  variations  can  be  considered  as 
the  controls  of  the  system  and  they  are  referred  to  as  shape  controls.  The  nonholonomic 
constraints  allow  us  to  express  the  global  motion  of  the  NVGT  assembly  as  a  function 
of  the  shape  and  of  the  shape  controls  and  to  formulate  motion  control  strategies  under 
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periodic  shape  controls. 

In  Chapter  4,  we  describe  G-Snakes ,  the  generalization  of  NVGT  assemblies  to 

nonholonomic  kinematics  chains  (or  other  kinematic  topologies)  on  arbitrary  Lie  groups 

(c.f.  (Krishnaprasad  &  Tsakiris  [1994a];  Krishnaprasad  &  Tsakiris  [1994c])).  G-Snakes 

are  t!-node  kinematic  chains,  where  each  node  evolves  on  a  Lie  group  G,  but  its  evolution 

is  subject  to  nonholonomic  constraints.  The  configuration  space  of  such  a  system  is 

Q  —  Ge  =  Gx-xG.  A  pair  of  nodes  constitutes  a  module  of  the  G-Snake,  whose 

' - - - -  ’ 

l  times 

shape  (i.e.  the  relative  configuration  of  the  two  nodes)  also  evolves  on  the  same  Lie  group 
G.  We  consider  the  variations  of  these  shapes  as  the  controls  of  the  system,  assuming 
that  a  suitable  mechanism  is  available  for  this  purpose.  The  shape  space  S  is  G^-1  and 
is  such  that  Q  =  S  x  G. 

Systems  of  this  type  have  some  interesting  geometric  properties.  First,  the  manifolds 
Q  and  5,  the  group  G  and  the  projection  n  :  Q  — >  S  form  a  principal  fiber  bundle 
(Bleecker  [1981];  Nomizu  [1956]).  Second,  in  the  case  that  the  number  of  nonholonomic 
constraints  is  equal  to  the  dimension  of  G,  the  constraints  specify  a  connection  on  the 
above  principal  fiber  bundle.  In  particular,  the  intersection  of  the  constraint  distribution 
Vq  at  a  point  q  £  Q  and  of  the  tangent  space  TqOrb(q)  to  the  orbits  of  the  action  of  G 
on  Q  at  q  is  trivial,  while  the  direct  sum  of  the  two  spaces  is  the  tangent  space  TqQ. 
Physically,  this  provides  a  splitting  of  the  velocities  of  the  G-Snake  in  two  groups:  one  is 
related  to  shape  variations  and  the  other  is  related  to  the  global  motion  of  the  system,  as 
it  is  characterized  by  a  group  trajectory.  The  connection  that  the  kinematic  constraints 
specify,  expresses  the  second  group  of  velocities  as  a  function  of  the  first  and  of  the  shape 
itself.  Notice  that  this  result  and  the  motion  control  schemes  to  which  it  gives  rise,  are 
entirely  based  on  kinematics. 

In  Chapter  6,  finally,  we  consider  the  dynamics  of  G-Snakes.  A  particular  instance 
of  the  case  when  the  number  of  constraints  is  one  less  than  the  dimension  of  the  group, 
is  presented,  namely  a  2-node  system  evolving  on  the  3-dimensional  group  SE{ 2)  with 
the  shape  space  being  S1.  The  previous  kinematic  motion  control  is  not  feasible  here, 
since  there  are  not  enough  constraints  to  specify  the  principal  fiber  bundle  connection. 
Even  though  the  sum  of  the  spaces  Vq  and  TqOrb(q)  is  the  tangent  space  TqQ,  the 
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intersection  of  the  two  spaces  is  now  non-trivial.  It  is  possible  however  to  use  the  system’s 
Lagrangian  dynamics  to  complement  the  kinematics.  The  notion  of  the  nonholonomic 
momentum,  introduced  in  (Bloch,  Krishnaprasad,  Marsden  &  Murray  [1994]),  can  be 
used,  together  with  the  kinematic  nonholonomic  constraints,  to  synthesize  a  principal 
fiber  bundle  connection.  The  evolution  of  this  momentum  depends  only  on  the  shape 
variables  of  the  system,  therefore  the  connection  again  provides  a  way  of  specifying  the 
global  motion  of  the  system  (as  it  is  characterized  by  a  group  trajectory)  using  only  the 
shape  and  its  variation.  Successful  dynamic  motion  control  strategies  depend  now  on 
properly  varying  the  shape,  so  that  the  system  builds  up  momentum  (Krishnaprasad  & 
Tsakiris  [1995]). 

Numerical  simulation  results  are  shown  for  both  the  kinematic  and  the  dynamic 
motion  control  cases. 
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CHAPTER  TWO 


PRELIMINARIES 


2.1  Introduction 

In  this  chapter  we  collect  some  definitions  and  facts  from  the  theory  of  Elliptic 
functions,  from  Differential  Geometry,  from  the  theory  of  Lie  groups  and  Lie  algebras  and 
from  Geometric  Mechanics.  We  also  summarize  the  kinematics  of  parallel  manipulators, 
which  are  robotic  devices  based  on  closed  kinematic  chains  and  we  study  their  kinematic 
singularities. 


2.2  Elliptic  Functions  and  Integrals 

In  Complex  Analysis,  a  doubly  periodic  function,  the  ratio  of  whose  periods  is 
complex  and  which  is  regular  except  for  poles,  is  called  elliptic.  The  Jacobi  elliptic 
functions  defined  below,  have  a  real  period,  like  the  trigonometric  functions,  and  an 
imaginary  one,  like  the  hyperbolic  functions.  Those  functions  are  defined  as  the  inverses, 
in  a  sense  to  be  made  precise  below,  of  the  elliptic  integrals.  The  material  in  this  section 
is  drawn  from  (Byrd  &  Friedman  [1954];  Davis  [1962];  Lawden  [1989])  and  (Gradstein  & 
Ryzhik  [1980]). 

Definition  2.2.1  (Elliptic  Integrals  of  the  First  Kind) 

The  elliptic  integral  of  the  first  kind  is  defined  as: 

<t>  sin  <j> 

.  f  dO  f  dx  . 

F(fi,k)=  /  =  =  /  — _  2.2.1) 

J  yjl  -  k2  sin2  9  J  \/{l  -  £2)(1  -  k2x2) 


where  k  is  the  modulus  of  F  and  k'  —  \/l  —  k2  is  the  complementary  modulus  of  F.  The 


integral 


m)*MFihk)  =  j 


dx 


x2)(l-k2x2) 


is  called  the  complete  elliptic  integral  of 
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the  first  kind.  Consider  also  the  complementary  complete  elliptic  integral  of  the  first 
kind  K'(k)  =f  F(f,fc')  =  f  - - **  9  .  Observe  that  K'(k)  =  KM)  . 

V  '  2  0  Vi1-*  )(!-k  x  )  ^ 

If  the  nome  q  =  exp  (-7r^)  is  real  and  0  <  q  <  1,  then  0  <  k  <  1  and  0  <  k'  <  1  . 


Definition  2.2.2  (Elliptic  Integrals  of  the  Second  Kind) 

The  elliptic  integral  of  the  second  kind  is  defined  as: 

<f>  sin  <f) 

E{4 ),  k)  =  [  Vl-k2  sin2  OdO  =  f  ^lE-dx  .  (2.2.2) 

J  J  v  1  —  x2 

0  0 

The  integral  E(k)  d=  E(^,k)  is  called  the  complete  elliptic  integral  of  the  second  kind. 

Consider  also  the  complementary  complete  elliptic  integral  of  the  second  kind  E'(k) 
E(^,k').  Observe  that  E'(k )  =  E(k')  . 

The  complete  and  complementary  complete  elliptic  integrals  of  the  first  and  second 
kind  are  related  by  Legendre’s  relation  EK'  +  E'K  —  KK'  =  |. 
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Definition  2.2.3  (Elliptic  Integrals  of  the  Third  Kind) 
The  elliptic  integral  of  the  third  kind  is  defined  as: 


<p 

n(</>,  v,  k)  =  J 


do 


sin  <t> 


-I  03 


dx 


(1  -  r)  sin2  6)  \/l  -  k2  sin2  9  J  (1  -  r?x2)v/(l  -  x2){\  -  k2x2) 


(2.2.3) 


The  first  form  of  F,  E  and  II  is  the  Legendre  normal  form  and  the  second  is  the 
Jacobi  normal  form. 

Definition  2.2.4  (Jacobi  Elliptic  Functions) 

The  Jacobi  amplitude  function  am(ti,  k)  is  defined  as  the  solution  for  (f)  of 

u  =  F{4>,k)  (2.2.4). 
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The  Jacobi  elliptic  functions  sn,  cn  and  dn  are  then  defined  as  follows  (c.f.  fig.  2.2.1): 


sn(n,  k)  =  siiuj)  =  sin(am(n,  k ))  , 

cn(n,  k)  =  cos  0  =  cos(am(n,  k ))  , 

^  =  dan^i)=  ,l_t2s.n 
au  du  v 

Nine  more  elliptic  functions  are  defined  by  taking  reciprocals  and  quotients: 


1 

ncu  = 

1 

ndn  = 

1 

snu  ’ 

enn  ’ 

dnn 

snu 

cdu  = 

enn 

dsn  = 

dnn 

cn  u  ’ 

dnn  ' 

snn 

cnu 

den  = 

dnn 

sdn  = 

snn 

snu  ’ 

enn 

dnn 

Then 


(2.2.6) 


u 


=  F((j),  k)  =  sn  1  (sin </>,  k)  =  cn  1  (cos  </>,  fc)  =  dn  1(\Jl-  k2  sin 2  0,  k) 


(2.2.7) 


=  am  k)  =  sc  ^tan^),  k)  . 


If  the  nome  q  is  real,  the  elliptic  functions  are  all  real,  for  all  real  values  of  their  parameter 
u. 


The  dependence  of  the  Jacobi  elliptic  functions  on  the  modulus  k  is  frequently 
suppressed. 
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Fig.  2.2.1:  Jacobi  Elliptic  Functions 


Proposition  2.2.5  (Special  Values) 

If  u  is  real,  then  —  1  <  snu  <  1,  —  1  <  cnu  <  1  and  k'  <  dnu  <  1.  Moreover, 


am(-u)  =  —  am(u)  ,  am(0)  =  0  ,  am (K)  =  —  ,  am(u  +  K)  —  tan  1(k'  scu)  +  —  , 

z  z 

sn(-ti)  =  -  sn(it)  ,  sn(0)  =  0  ,  sn(Ff)  =  1  ,  sn (u  +  K)  =  cd(u)  , 
cn (-it)  =  cn(u)  ,  cn(0)  =  1  ,  cn (K)  =  0  ,  cn {u  +  K)  =  —k'  sd(u)  , 
dn(-u)  =  dn (u)  ,  dn(0)  =  1  ,  dn(if)  =  k!  ,  dn (u  +  K)  =  k!  nd(u)  . 

(2.2.8) 

Some  special  values  of  the  modulus  are  as  follows: 


sn(u,  0)  =  sinu  ,  cn(u,0)  =  cosu  ,  dn(u,  0)  =  1  , 
sn(n,  1)  =  tanhu  ,  cn(u,  1)  =  dn(u,  1)  =  sechu  . 


(2.2.9) 


Proposition  2.2.6 


sn2  u  +  cn2  u  =  1  , 

dn2  u  +  k2  sn2  u  =  1  ,  (2.2.10) 

dn2  u  —  k2  cn2  u  =  k'2  . 
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Also 


sn-1  x  +  cd_1  x  =  K(k)  , 

cn-1  x  +  sd-1(-j^-)  =  K(k)  ,  (2.2.11) 

nd-1  x  +  dn ~1(k'x)  =  K(k)  . 


Proposition  2.2.7  (Addition) 


sn(u  ±v)  = 
cn(n  ±  v)  = 
dn(u  ±  v)  = 


sn  u  cn  v  dn  v  ±  sn  v  cn  u  dn  u 
1  —  k2  sn2  u  sn2  v 

cn  u  cn  v  ^  sn  u  sn  v  dn  u  dn  v 
1  —  k2  sn2  u  sn2  v 

dn  u  dn  v  ^  k2  sn  u  sn  v  cn  u  cn  v 
1  —  k2  sn2  u  sn2  v 


(2.2.12) 


■ 


Proposition  2.2.8  (Periodicity) 

The  elliptic  functions  have  two  distinct  periods  and,  if  the  nome  is  real,  one  period 
is  real  and  the  other  is  purely  imaginary.  The  periods  of  snu  are  4 K  and  2iK'  ,  those 
of  cn«  are  4 K  and  2 K  +  2 iK'  and  those  of  dn  u  are  2 K  and  4 iK'  .  Thus: 

sn (u  +  4 K)  =  sn(it  +  2iK')  =  snu  ,  sn (u  +  2 K)  =  —  sn  u  , 

cn(u  +  4 K)  =  cn(u  +  2 K  +  2iK')  =  cn  u  ,  cn (u  +  2 K)  =  -cnw,  (2  2  13) 

dn(u  +  2 K)  =  dn(u  +  4 iK')  =  dnu  ,  dn (u  +  K)  =  —  . 

dnu 


Proposition  2.2.9  (Derivatives,  Integrals  and  Differential  Equations) 


snu  =  cnudnu  ,  dnu  =  —  k2  snucnu  , 

du  du 


d 

du 


cn  u  =  —  sn  u  dn  u  ,  am  u  =  dn  u  , 

du 
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(2.2.14) 


J  snudu  =  i  ln(dnu  -  kcnu)  ,  J  sn 2  udu  =  -j^[u  —  E(amu,  k)]  , 
J  cnudu  =  ^  sin_1(A;snu)  ,  J  cn2  udu  =  [£(amu,  k)  —  k'2u] 
J  dnudu  —  sin_1(snu)  ,  J  dn2  udu  —  E(am  u,  k)  , 

Jsnucnudu  =  -l2dnu,  J  sn  u  dn  u  du  —  —  cn  u  , 

/ 


cn  u  dn  u  du  =  sn  u 


The  Jacobi  elliptic  functions  are  solutions  of  the  following  first-order  differential  equa¬ 
tions: 

d 


—  sn u  —  \/(l  —  sn2  u)(l  —  k2  sn2  u )  , 


—  cn u  =  —  \J (l  —  cn2  u)(k'2  +  k2  cn2  u)  , 
dn  u  =  —  \J (l  —  dn2  u)(dn2  u  —  k'2)  , 
and  of  the  following  second-order  differential  equations: 


(2.2.15) 


d2 

— -=■  snu  =  —(1  +  k 2)  snu  +  2 k2  sn3  u  , 
duz 


cnu  =  (2k2  —  1)  cnu  —  2 k2  cn3  u  , 


<£_ 

du 2 

d2 

—z  dnu  =  (2  —  k2)  dnu  —  2dn3  u  , 
du 1 


(2.2.16) 


Proposition  2.2.10  (Approximations) 

For  k  <C  1,  we  can  approximate  the  Jacobi  elliptic  functions  as  follows: 

sn(u,k)  «  sinu  —  k2  cos  u(u  —  sin  u  cos  u) /4  , 

cn (u,  k)  «  cosu  +  k 2  sinu(iz  —  sinucosu)/4  ,  (2.2.17) 

dn(u,  fc)wl  -  ( k 2  sin2  u)/2  . 
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For  k  «  1,  we  can  approximate  the  Jacobi  elliptic  functions  as  follows: 
sn(u,  k )  «  tanh  u  +  k'2  sech2  u(sinh  u  cosh  u  —  u)/ 4  , 

cn(u, A)  w  sech  u  —  k' 2  tanh  u  sech  u(sinhu  cosh  u  —  u)/4  ,  (2.2.18) 

dn(u,  k)  ~  sechu  +  fc/2  tanh  u  sech  u(sinhu  cosh  u  +  «)/ 4  . 


Proposition  2.2.11  (Elliptic  Integrals  of  the  First  Kind) 

Assume  u  and  k  are  real  with  0  <  k  <  1.  Let  u  =  sn_1(:r,  k ),  then  x  —  snu.  Observe 
that  sn_1(a:,  k)  is  a  multivalued  function  (since  sn(u+4AT)  =  snu  and  sn(2K —u)  —  snu). 
Restricting  to  0  <  x  <  1  and  the  corresponding  range  0  <  u  <  K,  from  the  Legendre 
canonical  form  of  the  elliptic  integral  of  the  first  kind,  we  get: 


sn 


X 

^(x,k)  =  u  —  J 


dt 


v/(l-t2)(l-*2*2) 


(2.2.19) 


The  following  elliptic  integrals,  which  can  be  evaluated  in  terms  of  the  Jacobi  elliptic 
functions,  are  also  reducible,  by  a  suitable  change  of  variables,  to  equation  (2.2.19): 


X 

I 


dt  1  _ . ,  x  b 

.  =  =  - sn 

v/(a2-i2)(&2-*2)  a  Va'’ 


(2.2.20) 


where  0  <  x  <  b  <  a  , 


O 

S  \/Ta 


dt 


1  -ux  b  \ 

:Cn  (r.  /  o  .  ,o) » 


^/(a2  +  f2)(62  -t2)  Va2  +  b 2  V  Va2  +  b2 ' 

where  0  <  £  <  6  , 


(2.2.21) 


0 

/  \/(a 


dt 


1  j-1/® 

=  -cd  (r>-)> 


i/(a2  —  t2)(b2  —  t2)  a  Kb'a 


(2.2.22) 


where  0  <  x  <  b  <  a  , 


X 

I  \f[a 


dt 


\J  (a2  +  t2)(b2  —  t2)  y/a?  +  b2 


1  ,_i,\/n2  +  b2  x  b  .  .  . 

sd  ( - IT - (2-2.23) 


ab  ’  v^a2  +  62 

where  0  <  x  <  b  , 
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I 


dt 


s/{t2  -  a2)  {t2  -  b2) 


1  ^  -i/x  b\ 
-dc 

a  a  a 


where  b  <  a  <  x  , 


OO 

/ 


dt 


^(t2  -  a2)(t2  -  b2) 


1  -i,x  bs 

-ns  , 

a  a  a 


where  b  <  a  <  x  , 


X 

/ 


dt 


1  ,-i,x  Va2  -  b2 

r  nd  (t. - : - )> 


\/(a2  —  t2)(t2  —  b2)  a  V  a 


where  b  <  x  <  a  , 


a 

I 


dt 


v/(a2  -t2)(t2  -b2) 


1  ,  _i.x  \/a2  -b2 

-dn  - ), 

a  a  a 


where  b  <  x  <  a  , 


X 

/ 


OO 

/ 


X 

/ 


OO 

/ 


dt 


1  x  b 

nc  ■■  , 

'  „  ‘  /  *5 To  '  5 


^/(t2  —  a2)(t2  +  b 2)  Va2  +  52  a  ’  \J a2  -\-b2‘ 

where  a  <  x  , 

.  1  ,  1  ,  b  ) , 

a/ (t2  —  a2)(t2  +  b 2)  Va2  +  62  \/ a2  +  b2  Va2  +  b2 

where  a  <  x  , 


dt 


1  i  x  \/a2  ~  b2 
7SC  (t. - : - )  > 


^(t2  +  a2)(t2  +  b2)  a  V  a 


where  0  <  b  <  a  ,  0  < 


dt 


7(t2+a2)(t2  +  h2) 


1  Va2  b2 . 

-cs  i(-, -  , 

a  a  a 


(2.2.24) 


(2.2.25) 


(2.2.26) 


(2.2.27) 


(2.2.28) 


(2.2.29) 


(2.2.30) 
x  , 

(2.2.31) 


where  0  <  b  <  a  ,  0  <  x  . 


2.3  Differential  Geometry  and  Lie  Groups 

In  this  section  we  define  Lie  groups  and  Lie  algebras  and  discuss  some  of  their 
properties  that  make  them  relevant  to  the  study  of  mechanical  systems  with  constraints 
and  with  symmetries.  The  material  in  this  section  is  drawn  from  (Abraham  &  Marsden 
[1985];  Boothby  [1975];  Curtis  [1984];  Jacobson  [1979];  Lang  [1985];  Marsden  &  Ratiu 
[1994];  Olver  [1986];  Spivak  [1970];  Varadarajan  [1984];  Warner  [1971]). 

Definition  2.3.1  (Group) 

A  group  ( G ,  •)  is  a  set  G,  together  with  an  operation  •  such  that  for  any  g,h  £  G, 
the  product  g  ■  h  is  also  an  element  of  G  and  such  that  the  following  properties  hold: 

i)  Associativity:  For  f,g,h  £  G  :  f  ■  (g  ■  h)  =  (/  •  g)  •  h 

ii)  Identity:  There  is  an  element  e  £  G  such  that  for  all  g  £  G  :  g  ■  e  =  e  ■  g  =  g 

iii)  Inverse:  For  each  g  £  G  there  is  an  element  of  G,  denoted  g~l  such  that  g  ■  g~l  = 
Q~l  -  9  =  e 


Let  G  and  G'  be  groups.  A  map  (f)  :  G  — >  G'  is  a  homomorphism  if  4>(g  ■  g')  = 
<f>{g)  ■  </>(<?').  A  homomorphism  is  surjective  (or  onto),  when  4>{G)  =  G' .  It  is  injective  (or 
one-to-one),  when,  if  4>(g)  =  4>{g'),  then  g  =  g' ■  A  necessary  and  sufficient  condition 
for  this  to  happen  is  that  </>_1(e')  =  e.  A  homomorphism  is  bijective,  if  it  is  onto  and 
one-to-one.  A  map  (j)  :  G  — >  G'  is  an  isomorphism  if  it  is  a  bijective  homomorphism. 
A  map  (f> :  G  — >  G  is  an  automorphism  if  it  is  an  isomorphism. 

Let  X  and  Y  be  smooth  vector  fields  on  a  manifold  M.  Let  Xm,Ym  £  TmM,  for 
m  £  M.  For  a  smooth  function  /  :  M  — >  JR,  the  operator  /  — >  Xm(Y(f))  does  not 
define,  in  general,  a  tangent  vector  at  m.  However,  XY  —  YX  does  (Boothby  [1975]). 
Definition  2.3.2  (Jacobi-Lie  bracket  of  vector  fields) 

Let  X  and  Y  be  smooth  vector  fields  on  a  differentiable  manifold  M.  We  define  a 
new  vector  field  [A,  Y],  called  the  Jacobi-Lie  bracket  of  X  and  Y  by 

IX,  Y]m(f)  =  Xm(Y(f))  -  Ym(X(f))  .  (2.3.1) 

for  m  G  M  and  for  all  smooth  functions  /  :  M - !>  JR. 
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For  a  smooth  vector  field  X  on  G,  let  (j>g  :  M  ->■  G ,  t  i->  <f>g(t)  be  a  tangent  curve 
of  X  at  a  fixed  point  g  €  G  ,  i.e.  a  smooth  curve  on  G  such  that  <f>g{ 0)  =  g  and 

Tt\t= 0^9 (*)  =  x(a)  • 

Proposition  2.3.3 

Consider  two  curves  and  <t>2g  tangent  to  the  smooth  vector  fields  X\  and  X2  respec¬ 
tively,  at  the  point  g  G  G  .  Then:  [XltX2](f)(g)  =  ft  ^[X2{4%(t))-Xi(<fi(t))](f){g)  . 

■ 

Definition  2.3.4  (Lie  Group) 

A  Lie  group  is  a  finite-dimensional  smooth  manifold  G  that  is  also  a  group  and  for 
which  the  group  operations  of  multiplication  •  :  G  x  G  — >  G  :  ( g,h )  g  ■  h  and  inversion 
-1  :  G  — >  G  :  g  g~l  are  smooth.  Let  e  be  the  group  identity. 

The  definitions  of  homomorphisms,  isomorphisms  and  automorphisms  of  Lie  groups 
are  similar  to  the  ones  for  general  groups,  except  that  the  maps  <f>  should  be  smooth. 
Then,  if  <j)  is  an  isomorphism,  it  is  a  diffeomorphism.  If  G'  =  Aut(V),  where  Aut(F) 
is  the  set  of  linear  nonsingular  operators  (automorphisms)  on  a  vector  space  V  or  if 
G'  =  Gl(n,  1R),  then  a  homomorphism  (f> :  G  — >  G'  is  called  a  representation  of  the  Lie 
group  G. 

Definition  2.3.5  (Direct  and  Semidirect  Products  of  Lie  Groups) 

The  direct  product  G  x  H  of  two  Lie  groups  G  and  H  is  a  new  Lie  group  with  the 
product  manifold  structure  and  the  direct  product  structure,  which  is  given  for  <7i ,  <72  €  G 
and  hi,  h2  G  H  by 

(<?ii  ^1)  '  (92,  h2 )  =  ( gi  ■  g2,h\  ■  h2 )  .  (2.3.2) 

For  Lie  groups  G  and  H,  suppose  G  acts  on  if  as  a  group  of  transformations  via 
h  i->  g  ■  h,  for  h  E  H  with  g  ■  (hi  ■  h2 )  —  (g  ■  hi)  ■  ( g  ■  h2).  The  semidirect  product  G  X5  H 
of  G  and  H  is  the  Lie  group  whose  manifold  structure  is  the  Cartesian  product  G  x  H, 
but  whose  group  multiplication  is 

(9i,  hi)  ■  (g2,h2)  =  (gi  ■  g2,  hi  ■  (gx  ■  h2))  .  (2.3.3) 
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For  example,  the  Special  Euclidean  group  SE(n),  i.e.  the  group  of  rigid  motions  in 
IRn,  is  the  semidirect  product  of  the  Special  Orthogonal  group  SO(n)  with  the  vector 
space  lRn,  where  SO(n)  acts  on  TRn  as  a  group  of  rotations,  with  group  multiplication 
(91,  hi)  ■  (92,  h2)  =  ( gi  -g2,hi  +9i  ■  h2),  for  31,32  G  SO(n)  and  huh2  €  lRn . 

Definition  2.3.6  (Lie  Algebra) 

A  Lie  algebra  is  a  real  vector  space  Q  equipped  with  a  product  Q  x  Q  — >  Q 
with  the  following  properties: 

i)  Bilinearity:  For  a,b  G  1R  and  x,  y,  z  G  Q  :  [ax  +  by,  z ]  =  a[x,  z]  +  b[y,  z],  [z,  ax  +  by)  = 
a[z,x]  +  b[z,y\. 

ii)  Skew-symmetry:  For  x,y  6  Q  :  [x,y}  =  —  [y,  xj. 

iii)  Jacobi  identity:  For  x,y,z  G  Q  :  [x,  [y,  z]\  +  [y,  [z,  x]  +  [z,  [x,  y]]  =  0. 


If  Q  and  Q'  are  Lie  algebras,  a  map  tp  :  Q  — >  Q'  is  a  (Lie  algebra)  homomorphism,  if 
it  is  linear  and  it  preserves  the  bracket,  i.e.  Y])  —  [tp(X),'4>(Y)},  for  every  X,  Y  6  Q. 

If,  in  addition,  xp  is  a  bijection,  then  it  is  an  isomorphism.  An  isomorphism  of  Q  with  itself 
is  an  automorphism.  If  Q'  =  End(Y),  the  set  of  all  linear  operators  (endomorphisms) 
on  a  vector  space  V  or  if  Q'  =  gl(n,lR),  then  a  homomorphism  xp  :  Q  — >  Q'  is  called  a 
representation  of  the  Lie  algebra  Q. 

A  subspace  B  of  a  Lie  algebra  Q  is  a  subalgebra  if  [x,  y]  G  B  for  all  x,  y  6  B.  It  is  an 
ideal  if  [x,y]  €  B  for  all  x  €  Q  and  y  6  B.  Let  B\  and  B2  be  subspaces  of  a  Lie  algebra 
Q  and  denote  by  [Bi,B2]  the  span  of  all  elements  [bi,b2)  such  that  61  G  B\  and  b2  G  B2- 
Define  the  derived  series  of  Q  by 

gog'  =  [g,  g]  d  g"  =  [g1, g']  d  •  •  o  g^  =  ^(*-1), d  . . . 

Its  terms  are  ideals.  So  are  the  terms  of  the  loxver  central  series  of  g  defined  by 

g^g2  =  [ g , g]  2  gz  =  \g2,g]  ^■■■^gk  =  [gk~\g\  2  •  ■  ■ 

Definition  2.3.7  (Solvable  Lie  Algebras) 

A  Lie  algebra  Q  is  solvable  if  =  0,  for  some  positive  integer  n. 
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Abelian  algebras  are  solvable.  All  Lie  algebras  Q  with  dim Q  <  3  and  dim*?'  <  3  are 
solvable  (e.g.  se( 2)).  Also,  the  algebra  of  triangular  matrices  is  solvable. 

Definition  2.3.8  (Nilpotent  Lie  Algebras) 

A  Lie  algebra  Q  is  nilpotent  if  Qn  =  0,  for  some  positive  integer  n. 


The  algebra  h(n)  of  nil  triangular  n  x  n  matrices  (triangular  matrices  where  also  the 
diagonal  is  zero)  is  nilpotent.  If  Q  is  nilpotent,  then  it  is  also  solvable  (since  Q C  Q2*). 
The  converse  does  not  hold. 

Definition  2.3.9  (Abelian  and  Non-Abelian  Lie  Algebras) 

A  Lie  algebra  Q  is  abelian  if  Q'  —  0.  Otherwise,  it  is  called  non-abelian. 


Definition  2.3.10  (Simple  Lie  Algebras) 

A  Lie  algebra  Q  is  simple  if  it  is  non-abelian  and  its  only  ideals  are  0  and  Q. 


■ 


Definition  2.3.11  (Semi-Simple  Lie  Algebras) 

A  Lie  algebra  Q  is  semi-simple  if  its  only  abelian  ideal  is  0. 


Simplicity  implies  semi-simplicity.  All  3-dimensional  Lie  algebras  such  that  dim Q'  =  3 
(i.e.  Q'  =  Q)  are  simple  (e.g.  so(3),  sZ(2)). 

Definition  2.3.12 

i)  The  left  and  right  translation  by  g  G  G  are  the  maps  Lg  :  G  — »  G  :  h  h4  gh  and 
Rg  :  G  G  :  h  »->■  hg.  Both  are  diffeomorphisms. 

ii)  A  vector  field  X  on  G  is  called  left-invariant  if  for  every  g  6  G,  (L9)*X  =  X, 
where  (Lg)t.  is  the  differential  of  Lg.  Equivalently,  ThLgX(h )  —  X(gh),  for  every  h  £  G, 
where  T^Lg  is  the  linearization  of  the  map  Lg  at  h. 


The  set  of  smooth  vector  fields  on  a  manifold  M,  with  the  [., .]  bracket  defined  in 
equation  (2.3.1),  is  a  Lie  algebra  (possibly  infinite-dimensional).  When  M  is  a  Lie  group, 
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then  this  Lie  algebra  has  an  important  finite-dimensional  subalgebra,  which  is  called  the 
Lie  algebra  of  M  and  is  defined  below. 

Consider  a  Lie  group  G.  For  £  £  TeG,  define  on  G  the  left-invariant  vector  field 
X{(g)  =  TeLg£,  for  g  £  G.  Then,  TeG ,  together  with  the  Lie  bracket  [., .]  defined  by 

[£,*?]  =  pQ,X„](e),  (2.3.4) 

for  £,7?  £  TeG ,  becomes  a  Lie  algebra.  Moreover, 

Definition  2.3.13  (Lie  Algebra  of  Lie  Group) 

The  vector  space  TeG  with  the  Lie  bracket  [., .]  defined  in  equation  (2.3.4)  is  called 
the  Lie  algebra  of  G  and  denoted  Q  (or  £(G)). 


For  £  £  Q,  let  <j>^  :  IR  — >  G  :  t  exp  f£  denote  the  one-parameter  subgroup  of  G, 
i.e.  the  integral  curve  of  the  left-invariant  vector  field  X ^  passing  through  the  identity 
at  t  =  0. 

For  matrix  Lie  groups  G  we  have  for  g  £  G  and  £  £  Q  : 


TeLg  ■  £  —  L9£  —  (/£  . 


(2.3.5) 


Moreover,  for  h  £  G  and  for  Xh  £  ThG  we  have: 

ThLg  :  ThG  —7  TghG 

Xh  ThLg  ■  Xh  —  LgXh  =  gXh 


(2.3.6) 


Consider  a  left-invariant  dynamical  system  on  a  matrix  Lie  group  G  with  n- 
dimensional  Lie  algebra  Q.  Consider  a  curve  g(.)  C  G.  Then,  there  exists  a  curve  £(.)  £  Q 
such  that: 

g  =  TeLg  ■  £  =  Lgi  =  gi  .  (2.3.7) 

Let  t  =  l,...,n}  be  a  basis  of  Q  and  let  [., .]  be  the  usual  Lie  bracket  on  Q 
defined  by:  [Ai,Aj\  —  AiAj  —  AjAi.  Then,  there  exist  constants  r*  •,  called  structure 
constants,  such  that: 


[Ai,Aj\  =  ^2  rtjAk  ,  i,j  =  1,.  ,n.  (2.3.8) 

fc= l 
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Let  Q*  be  the  dual  space  of  G,  he.  the  space  of  linear  functions  from  Q  to  JR.  Let 
{•4.J,  i  =  1, . . . ,  n}  be  the  basis  of  Q*  such  that 

A\(Aj)^6i  i,j  =  1, . . . ,  n,  (2.3.9) 

where  5j  is  the  Kronecker  symbol.  Then  the  curve  £(.)  C  G  can  be  represented  as: 

n  n 

£  =  =  (2.3.10) 

1=1  i= 1 


for  £  E  1R,  i  =  1, ...  ,n. 

I 

To  obtain  a  solution  of  the  dynamical  system  (2.3.7)  we  use  the  following  product- 
of-exponentials  representation. 


Proposition  2.3.14  (Wei  &  Norman  [1964]) 

Let  g(0)  =  /,  the  identity  of  G  and  let  g(t)  be  the  solution  of  (2.3.7).  Then,  locally 
around  t  =  0,  g  is  of  the  form: 

g(t)  =  e7i(*Mie7a(tM*  . . .  e7»(t)^n  )  (2.3.11) 


where  the  coefficients  7j  are  determined  by  differentiating  (2.3.11)  and  using  (2.3.7).  For 
the  coordinates  £.  of  £  €  G  defined  in  (2.3.10),  we  get: 

i 


(  7i  ^ 

rM 

=  M(7i  , . 

••,7n) 

\7tJ 

(2.3.12) 


The  matrix  M  is  analytic  in  7  and  depends  only  on  the  Lie  algebra  G  and  its  structure 
constants  in  the  given  basis.  If  G  is  solvable,  then  there  exists  a  basis  of  Q  and  an  ordering 
of  this  basis,  for  which  (2.3.1  l)is  global.  Then  the  7j’s  can  be  found  by  quadratures. 


Definition  2.3.15  (Action  of  Lie  Group  on  Manifold) 

Let  Q  be  a  smooth  manifold.  A  (left)  action  of  a  Lie  group  G  on  Q  is  a  smooth 
mapping  $:GxQ  — >  Q  such  that: 
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i)  For  all  q  €  Q,  $(e,x)  =  x. 

ii)  For  every  g,h  G  G,  $(g,$(h,q))  =  <&(gh,q),  for  all  q  EQ. 


For  every  g  €  G,  define  :  Q  — >  Q  :  q  — >  $(g,q).  Then  from  ( i ),  4>e  =  id,Q  and 
from  (ii),  $gh  =  o  Moreover,  ($9)_1  =  $g-i.  Thus,  4>g  is  a  diffeomorphism  (i.e. 
one-to-one,  onto  and  both  <f>g  and  (<&5)_1  are  smooth). 

Definitions  2.3.16 

Let  $  be  an  action  of  G  on  Q. 

i)  For  q  6  Q,  the  orbit  (or  <£>-orbit)  of  q  is  Orb (q)  =  {<Fg((7)|<7  €  G}. 

ii)  An  action  is  transitive  if  there  is  only  one  orbit. 

iii)  An  action  $  is  effective  (or  faithful)  if  g  *->■  <J>S  is  one-to-one. 

iv)  An  action  $  is  free  if,  for  each  q  £  Q,  g  1-4  &g(q)  is  one-to-one,  i.e.  the  identity  e  is 
the  only  element  of  G  with  a  fixed  point. 

v)  An  action  <&  is  proper  if  and  only  if  the  map  4>  :  G  x  Q  — »  Q  x  Q  :  (g,q)  ■-> 
(q,$(g,q))  =  $(</,  q)  is  proper,  i.e.  if  K  C  Q  x  Q  is  compact,  then  •h-1^)  is  compact. 


Definition  2.3.17  (Infinitesimal  Generator) 

Let  $  :  GxQ  — >  Q  be  a  smooth  action.  If  £  e  Q,  then  &  :  IRxQ  — *•  Q  :  ( t ,  q)  t-4 
<I>(expf£,  q)  is  an  JR-action  on  Q,  i.e.  is  a  flow  on  Q.  The  corresponding  vector  field  on 
Q  is  called  the  infinitesimal  generator  of  $  corresponding  to  £  and  is  given  by 


(Q(g)  =  ^$(exp  t£,q) 


t= 0 


(2.3.13) 


Then,  the  tangent  space  to  the  orbit  Orb(g)  of  q  is 


T9Orb(g)  =  {£Q(g)|£  €  S}  .  (2.3.14) 


Examples  2.3.18 

i)  Let  $  be  the  left  translation  of  a  matrix  Lie  group  G,  considered  as  an  action  of 
G  on  G,  i.e.  $:GxG  — >  G  :  (g,  h)  >->  gh  =  Lgh.  Then  the  infinitesimal  generator  £ 

G 

corresponding  to  £  £  Q  is  £  (g)  =  TeRg  ■  £  =  £5,  which  is  a  right  invariant  vector  field. 


23 


ii)  Consider  the  adjoint  action  of  G  on  its  Lie  algebra  G,  defined  as  $ 

Q  :  {g,g)  >-4  Adgg  =  Te(Rg-iLg)rj.  If  G  is  a  matrix  group,  then 

Adgr)  =  grjg-1  , 

for  g  G  Q.  If  i  G  G,  then 

tgV  =  adCV  =f  [C  rj\  •  (2.3.16) 

Further  define  adkg  =f  ad^(adk~1g)  =  [£ , adk ~ 1  ??] . 

The  following  properties  of  the  adjoint  action  of  a  matrix  Lie  group  are  easily  checked: 

i)  Adg(aigi  +  a2g 2)  =  aiAdgr)i  +  a2Adgg2 ,  for  a\,a2  €  1R  and  g\,g2  G  G- 

ii)  Adgi92(g)  =  Adgi(Ad92 g),  for  gug2sG. 

iii)  Adg (r}ig2)  =  AdggiAdgg2 ,  for  771 , 772  €  G- 

■ 


G  x  G 


(2.3.15) 


Definition  2.3.19  (Equivariance) 

Let  M  and  N  be  manifolds  and  G  a  Lie  group.  Let  $  and  'L  be  actions  of  G  on 
M  and  N  respectively  and  /  :  M  — >  N  be  a  smooth  map.  Then  /  is  equivariant  with 
respect  to  those  actions,  if  for  all  g  €  G  :  /  o  o  /. 

■ 


Proposition  2.3.20 

Let  /  :  M  — >  N  be  equivariant  with  respect  to  actions  $  and  T  as  above.  Then, 
for  any  £  G  G,  we  have  Tf  ■  (m  =  •  /,  where  and  £/v  are  infinitesimal  generators 

of  <I>  and  Vi'  respectively  on  M  and  N  corresponding  to  £. 


From  (2.3.10)  we  have: 

n  n 

=  E^(0^A  •  (2-3.17) 

i= 1  i=l 


From  (2.3.11)  we  have: 


=  gAig-1  =  e71*41  ■  ■  •  e^AnAie~^An  ■  ■  ■  e-71-41  (2.3.18) 


24 


and 

Adg-xAi  =  g~lAig  =  e~lnAn  ■  ■  ■  e~^Al  AiellM  ...e^An  (2.3.19) 

Equation  (2.3.18)  can  be  made  more  explicit  by  the  Baker-Campbell— Hausdorff  formula 
(Wei  &  Norman  [1964]),  which  for  G  Q  states  that: 

Adexp^ri  —  e^r)e~^  =  {eadi)g  =  ??  +  [£,  r/]  +  ^y[£,  [t,,g]}  +  ■  ■  ■  .  (2.3.20) 

Thus 

AdgAi  =  ead{llAl)  ■  ■  ■  ead('~1nAn') Ai  (2.3.21) 

and 

Adg-xAi  =  ead^nA^  •  •  ■  ead^M)Ai  ,  (2.3.22) 

for  i  =  1, . . .  ,n. 

The  following  material  on  principal  fiber  bundles  and  connections  is  based  on 
(Bleecker  [1981];  Nomizu  [1956]).  These  references  consider  principal  fiber  bundles  where 
the  group  action  is  a  right  action.  Here  we  consider  left  actions  and  modify  appropriately 
the  definition  of  a  principal  fiber  bundle  following  (Yang  [1992]). 

Definition  2.3.21  (Principal  Fiber  Bundle) 

Let  S  be  a  differentiable  manifold  and  G  a  Lie  group.  A  differentiable  manifold  Q 
is  called  a  (differentiable)  principal  fiber  bundle  if  the  following  conditions  are  satisfied: 

1)  G  acts  on  Q  to  the  left,  freely  and  differentiably: 


$  :  G  X  Q  ->■  Q  :  (g,q)  g  ■  q  =  •  q  . 

2)  S  is  the  quotient  space  of  Q  by  the  equivalence  relation  induced  by  G,  i.e.  S  =  Q/G 
and  the  canonical  projection  n  :  Q  — »  S  is  differentiable. 

3)  Q  is  locally  trivial,  i.e.  every  point  s  €  S  has  a  neighborhood  U  such  that  7r-1(f7)  C  Q 

is  isomorphic  with  U  x  G,  in  the  sense  that  q  £  7t_1(17)  (n(q),(f>(q))  6  U  x  G  is  a 

diffeomorphism  such  that  </> :  7t-1(£7)  — >  G  satisfies  <fi(g  •  q)  =  g4>{q),'ig  G  G. 

For  s  G  <S,  the  fiber  over  s  is  a  closed  submanifold  of  Q  which  is  differentiably  isomorphic 
with  G.  For  any  q  G  Q,  the  fiber  through  q  is  the  fiber  over  s  =  7r(g).  When  Q  =  S  x  G, 
then  Q  is  said  to  be  a  trivial  principal  fiber  bundle  (fig.  2.3.1). 
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Fig.  2.3.1:  Connection  on  a  Principal  Fiber  Bundle 

Definition  2.3.22  (Connection  on  a  Principal  Fiber  Bundle) 

Let  (Q,S,n,G)  be  a  principal  fiber  bundle.  A  connection  on  the  principal  fiber 
bundle  is  a  choice  of  a  tangent  subspace  Hq  C  TqQ  at  each  point  q  £  Q  (horizontal 

subspace)  such  that,  if  Vq  d=f  {u  €  TqQ\-K*q(v)  =  0}  is  the  subspace  of  TqQ  tangent  to 
the  fiber  through  q  (vertical  subspace),  we  have: 

1)  TqQ  =  Hq  ®  Vq. 

2)  For  every  g  €  G  and  q  G  Q,  Tq$g  ■  Hq  =  Hg.q. 

3)  Hq  depends  differentiably  on  q. 


2.4  Geometric  Mechanics 

The  material  in  this  section  is  drawn  from  (Abraham  &  Marsden  [1985];  Marsden, 
Montgomery  Kz.  Ratiu  [1990];  Marsden  &  Ratiu  [1994];  Marsden  [1991])  and  (Arnold 
[1978]). 
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Definition  2.4.1  (Poisson  manifold) 

Let  P  be  a  manifold  and  let  C°°(P)  be  the  set  of  smooth  real-valued  functions  on 
P.  Consider  a  bracket  operation 

C°°{P)  x  C°°(P)  — >  C°°{P)  . 

The  pair  (P,  {,})  is  called  a  Poisson  manifold,  if,  for  f,g,h  G  C°°(P),  the  bracket  {, } 
satisfies: 

1)  Anticommutativity  {/,<?}  =  —  {g, /}  . 

2)  Bilinearity  {/,  A g  +  gh}  =  A{/,  g }  +  g{f,  h}  ,  A,  g  G  1R  . 

3)  Jacobi’s  identity  {{/,  g},  h}  +  {{5,  h},  /}  +  {{h,  /},  g}  =  0  . 

4)  Leibnitz’s  rule  {fg,  h}  =  f{g,  h}  +  g{f,  h }  . 

Properties  (1),  (2)  and  (3)  above  make  C°°(P)  a  Lie  algebra  under  the  Poisson  bracket. 


A  Poisson  bracket  is  uniquely  associated  to  a  contravariant,  skew-symmetric  two- 
tensor  A  on  P  such  that: 

{f,9}  =  A (df,dg)  . 

If  P  is  a  finite  dimensional  manifold  of  dimension  n,  then  A  is  given  by  an  n  x  n  skew- 
symmetric  matrix  and  the  Poisson  bracket  can  be  expressed  in  local  coordinates  as 

{/,<?}  =  V/T A V<? . 

Definition  2.4.2  (Hamiltonian  Vector  Field) 

Let  (P,  {,  })  be  a  Poisson  manifold  and  let  /  G  C°°(P).  The  Hamiltonian  vector  field 
Xf  is  the  unique  vector  field  on  P  such  that 

^)  =  {9-/}.v?er(P).  (2-4.1) 


The  set  of  Hamiltonian  vector  fields  on  P  is  a  Lie  subalgebra  of  the  set  of  vector  fields 
on  P  and,  in  fact,  for  /,  g  G  C°°(P)  : 

[Xf,X,]=-XIM.  (2.4.2) 
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An  integral  curve  /i(.)  C  P  of  Xf  for  /  G  C°°(P)  satisfies  in  local  coordinates: 

,  i  =  1, . . .  ,n  .  (2.4.3) 

A  function  <p  G  C°°  ( P )  is  called  a  Casimir  function  if 

{</>,</>}  =0,Vt/>G  C°°(P).  (2.4.4) 

Such  a  function  is  constant  along  the  flow  of  all  Hamiltonian  vector  fields. 

Every  symplectic  manifold  is  Poisson.  However,  not  every  Poisson  manifold  is  sym- 
plectic.  An  important  class  of  non-symplectic  Poisson  manifolds  is  the  class  of  Lie- 
Poisson  manifolds ,  an  example  of  which  is  the  dual  space  Q*  of  a  Lie  algebra  Q. 

Let  < ,  >  be  the  natural  pairing  between  elements  of  the  Lie  algebra  Q  and  those 
of  its  dual  space  Q* .  Let  {A\,Ai,  ■  ■  ■  ,An}  be  a  basis  of  Q  and  let  {A\,A2,  ■  ■  ■  ,-4.^}  be 

the  dual  basis  of  Q*,  which  is  such  that  <  A\ ,  Aj  >—  6%,  where  Sj  is  the  Kronecker 

symbol.  Any  /j,  G  Q*  can  be  expressed  as  n  —  ^”=1  The  space  Q*  becomes  a 

Poisson  manifold  under  either  one  of  the  following  Lie-Poisson  brackets: 

=  <2'4'5> 

where  <f>, xp  G  C°°(Q*)  and  /i  G  Q* .  This  Poisson  manifold  will  be  denoted  G±-  The 
variational  derivative  G  G  is  defined  via  the  Frechet  derivative  by 

= Dm  • " = + ii,) = E  • for " s  • 

i=l 

Let  rj*  ■  be  the  structure  constants  of  Q  in  the  given  basis.  Then,  the  Lie-Poisson  bracket 
is  expressed  in  the  local  coordinates  for  Q  and  Q*  as: 


i= 1 j=l fe=l 


1=1 j= 1 


d(j)  dtp 

'  dfii  d/ij 


(2.4.6) 


where  A *=  ±  ^”=1  are  the  elements  of  the  skew-symmetric  matrix  A  corre¬ 

sponding  to  the  bracket  {,  }±. 
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2.5  Parallel  Manipulators 

In  section  2.5.1  we  examine  3-dimensional  (spatial)  parallel  manipulators.  We  de¬ 
velop  the  inverse  kinematics  and  the  velocity  kinematics  with  respect  both  to  the  spatial 
and  to  the  body  velocities.  We  define  the  notions  of  forward  and  inverse  velocity  maps 
and  singular  configurations  and  show  that  both  formulations  of  the  Jacobian  (both  with 
respect  to  the  spatial  and  to  body  velocities)  specify  the  same  set  of  singular  configura¬ 
tions. 

In  section  2.5.2  we  examine  2-dimensional  (planar)  parallel  manipulators.  In  addi¬ 
tion  to  the  above,  we  define  the  notion  of  singular  surfaces  on  the  configuration  space 
and  we  discuss  the  effect  of  singularities  on  an  active  robotic  system. 

2.5.1  Spatial  Parallel  Manipulators 

Consider  a  parallel  manipulator  of  the  Stewart  platform  type  (Fichter  [1986])  moving 
in  3-dimensional  space  (fig.  2.5.1). 


Fig.  2.5.1:  Stewart  Platform 


Let  Ob  and  Op  be,  respectively,  the  basis  and  platform  coordinate  systems.  Also 
let  5  =  ^  q"  E  G  =  with  R  £  50(3),  be  the  configuration  matrix  of  the 
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platform  with  respect  to  Ob,  Bi  be  the  position  of  the  i  —  th  basis  joint  with  respect  to 
Ob ,  pPi  be  the  position  of  the  i—th  platform  joint  with  respect  to  Op,  Pt  =  T  +  RpPi 
be  the  position  of  the  i  —  th  platform  joint  with  respect  to  Ob,  Si  =  Pi  —  Bi  the  vector 
of  the  i  —  th  leg  with  respect  to  Ob,  =  || •S'i || 3  be  the  length  of  the  i  —  th  leg,  a  G  1R 6 
be  the  vector  (0 1, . . . ,  ct6)t  and  x  =  P,  x  S,  =  B,  x  S.t  be  the  moment  of  St 

with  respect  to  Ob,  where  ||a;||n  =  ^/<  x,  x  >n  is  the  usual  Euclidean  norm  for  x  G  JR” 
(for  the  proper  n)  and  <,  >n  is  the  inner  product  for  lRn. 

Given  a  configuration  g  G  SE( 3)  of  the  platform,  the  inverse  kinematics  problem 
is  to  specify  the  corresponding  platform  leg  lengths  ou  i—  1, . . . ,  6  and  define  the  inverse 
kinematic  map  T~x  :  SE( 3)  — >  IR6  :  g  i->  a. 


The  homogeneous  coordinate  representation  of  the  vector  St  is 


(c.f.  (Murray, 


Li  &  Sastry  [1994])).  From  fig.  2.5.1  we  have: 


St  =  Pi  -  Bi  =  T  +  R  pPi  -  Bi 


(2.5.1) 


Then,  by  definition,  the  platform  leg  length  Oi  is  given  by: 


def 
Oi  = 


iiaiu 


Thus  the  inverse  kinematic  map  T  1  is: 


P-\9)  =  o(g) 


/nails 

\  naiu 


(2.5.2) 


(2.5.3) 


Given  a  set  of  platform  leg  lengths  cr*,  i  =  1, . . . ,  6,  the  forward  kinematics 
problem  is  to  specify  the  corresponding  platform  configuration  g  G  SE( 3)  and  define  the 
forward  kinematic  map  T  :  2R6  — >  SE(3)  :  a  g.  This  is  usually  much  harder  than 
inverse  kinematics  for  parallel  manipulators.  Issues  related  to  the  forward  kinematics 
problem  are  addressed  in  (Fichter  [1986];  Merlet  [1992];  Nanua  &  Waldron  [1989];  Nanua, 
Waldron  &  Murthy  [1990];  Tahmasebi  &  Tsai  [1991]). 
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Let  u)  be  the  skew-symmetric  matrix  corresponding  to  the  vector  u i 


[uA 

I  w2  I  €  1R3, 


0 

—0)  3 

U>2 

Ci>3 

0 

-CU1 

—U>2 

Wi 

0 

The  mapping  identifies  the  Lie  algebra  TeSO( 3) 


with  2R3.  Under  this  identification,  the  Lie  bracket  corresponds  to  the  cross  product  in 
1R 3  (Abraham  &  Marsden  [1985])  . 

Definition  2.5.1  (Spatial  and  Body  Velocities) 

The  spatial  angular  velocity  ill  is  defined  as  the  vector  corresponding  to  the  skew- 
symmetric  matrix  u  =  RRJ]  the  spatial  translational  velocity  £  as  £  =  T  —  RRrT  ; 
the  body  angular  velocity  Cl  as  the  vector  corresponding  to  the  skew-symmetric  matrix 
Cl  =  RT R  and  the  body  translational  velocity  SasS  =  RtT  . 


Lemma  2.5.2  (Left  and  Right  Translation  in  SE( 3)) 
For  g  E  SE( 3)  : 


9  = 


U!  £ 

0  0 


)9  =9(o  o)  ' 


Also,  u  =  RCl ,  <£>  =  AdjiCl  =  RCIRJ  ,  £  —  RE  —  RClRrT  . 

Proof 

( R  T\ 

By  differentiating  g  =  f  j  ,  we  have: 


(2.5.4) 


R  T 
0  0 


RRJ  T  -  RRtT 
0  0 


R  T 
0  1 


R  T\  ( RtR  RtT 

0  1  j  l  0  0 


The  result  follows  from  Definition  2.5.1. 


Let  [oi]i  denote  the  matrix  whose  i-th  row  is  a*,  let  S  denote  the  diagonal  matrix  with 
elements  {<7;,  i  =  1, . . . ,  6}  and  let  a  denote  the  vector  of  leg  velocities  {&i,  i  =  1, . . . ,  6}. 
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Theorem  2.5.3  (Velocity  Kinematics) 

The  spatial  velocities  (£,  u)  of  the  platform  and  the  leg  velocities  d*  of  its  legs  are 
related  as  follows: 

Y 


V[*)*  =  [Sil  | M>\x[g) 


iv 


(2.5.5) 


The  body  velocities  (H,  Cl)  of  the  platform  and  the  leg  velocities  a.L  of  its  legs  are 
related  as  follows: 


T 


X(a)a  =  [Si'R\SjRPPi  ],(<?) 


ci 


(2.5.6) 


Let  Jsp(g)  =  [Sj\Mj]i  and  JB(g)  =  [SjR\SjRpPi  },.  Also,  let  Jsv  =  S-1[S7|AfT]< 
and  JB  =  Z-'iSj R\SJ Rvf  }i. 

Proof 

From  (2.5.1)  by  differentiating: 


0 


1 


0  0 


CD- 


=  9 


ci  s\  (m 


o  o  \  1 


C+u(T  +  RPPi)'\  _  (i  +  CvPi 
0 


RE  +  RCl  PPi 
0 


Then 


Si  =  £  +  uPi  =  R(E  +  Cl  vPi )  . 


(2.5.7) 

(2.5.8) 


From  (2.5.3)  we  have  that  of  =<  Si,  Si  >3  .  By  differentiating  this  we  get: 

S(T  =  [c Ti&iji  =  [<  Si,  Si  >3]i  -  [57 Si]i  , 

Then  from  (2.5.7)  and  (2.5.8)  we  get: 

EA  =  [5,T  $],  =  [57  ((  +  (vPi)}i  =  [57e  +  Sj  PtT  <4 

=  [S?\MJ]ify  . 

For  the  second  part  of  the  theorem,  again  from  (2.5.7)  and  (2.5.8)  we  get: 

Ed  =  [Sj Si]i  =  [57 R(E  +  Cl  pPi)]i  =  [Sj RE  +  Sj RCl  PP^  =  [S? RE  -  Sj R^Cl]i 


=  [SjR\SjRpPi 


Cl 
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Corollary  2.5.4  (Forward  Velocity  Kinematics) 

At  a  platform  configuration  g  with  corresponding  leg  lengths  a,  the  spatial  forward 


velocity  kinematic  map  ( 


dJ7  \  .i.  i  I  O  K,..  (  £  l  _  'T-i 


:  a  h4 


u 


is  given  by: 


U! 


= 


The  body  forward  velocity  kinematic  map  ( 


:  a  i-» 


n 


is  given  by:  I  “  1  = 


Corollary  2.5.5  (Inverse  Velocity  Kinematics) 

At  a  platform  configuration  g  with  corresponding  leg  lengths  a,  the  spatial  inverse 


velocity  kinematic  map 


da 


it) 


er  is  given  by:  a  —  Jt 


sv 


The  body  inverse  velocity  kinematic  map 


da 


n 


a  is  given  by:  a  = 


I 


Definition  2.5.6  (Kinematic  Singularities) 

Platform  configurations  g,  where  Jsp  is  singular,  are  called  forward  kinematic  sin¬ 
gularities.  Platform  configurations  g,  where  £  is  singular,  are  called  inverse  kinematic 
singularities  . 


Issues  related  to  singular  configurations  of  parallel  manipulators  are  explored  in 
(Gosselin  &  Angeles  [1990];  Hunt  [1983];  Ma  &  Angeles  [1991];  Merlet  [1988];  Merlet 
[1989];  Merlet  [1988];  Sugimoto,  Duffy  &  Hunt  [1982]). 

The  forward  kinematic  singularities  were  defined  using  the  spatial  velocity  formula¬ 
tion.  It  can  be  shown  that  it  would  be  equivalent  to  define  them  using  the  body  velocity 
formulation. 

Proposition  2.5.7 

The  following  conditions  are  equivalent  for  a  configuration  g  €  SE(3)  to  be  a  forward 
kinematic  singularity 
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(1)  detJsp(g)  =  0. 

(2)  there  exists  (£,  a>)  /  0  such  that  <  Si,  £  +  CjPi  >3=  0,  for  all  t  =  1, . . . ,  6. 

(3)  detJB(g)  =  0. 

(4)  there  exists  (S,  Q)  ^  0  such  that  <  S,,R{E  +  ClpPi)  >3=  0,  for  all 

*  =  !»••■,  6. 

Proof 

(1)  (2)  :  Observe  that  (using  <  a  x  b,c  >3=<  b,cx  a  >3): 

Jsp  (f,)  =  [Sl\Mj]t  (j^=[sTt  +  (PiX  Si)Tu> }, 

=  [S?£  +  ^7 x  P)},  =  [57 (£  +  uPi)},  =  [<  Si,£  +  C bPi  >3]i 


Thus  if  there  exists  (£,w)  ^  0  such  that  JBp 


—  0  ,  then  <  Su£  +  uP,  >3= 


0  ,  Vi  =  1, . . . ,  6  and  vice-versa. 


(2)  •<=*>  (4)  :  Easy  to  see  from  Lemma  2.5.2  : 


<  Su  £  +  uPi  >3  =  57  (£  +  u>Pi)  =  Sj  {RE  -  RQRtT  +  RClRT P,) 

=  Sj R[E  +  OR7  {Pi  -  T)]  =  Sj R[E  +  ttRT R  pPi )]  . 
=  57 R{ E  +  Cl  vPi)  =<  Si,  R{ E  +  ClpPi)  >3 


(3)  (4)  :  Observe  that: 

Jb  (  ^  )  =  [Sj RE  +  {R  pPi)TRtt}i  =  [57 RE  +  Sj {Rn)  x  {R  ^)]i 
=  [57 RE  +  57 RQR  pPi]i  =  [57 R{ E  +  h  pP,)]i 

■ 


Corollary  2.5.8 

If  we  start  at  a  singular  configuration  g *  G  SE{3),  the  input  a  G  1R6  determines 
the  platform  motion  only  up  to  an  element  of  the  null  space  A f9t{Jsp)  of  Jsp{g*)- 
This  element  corresponds  to  such  motions  that  make  the  velocity  of  all  joints  Pi  either 
perpendicular  to  the  corresponding  leg  vector  S,  or  zero. 
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Proof 


If  we  lock  the  legs  of  the  platform  (a  =  0)  and  apply  Corollary  2.5.5  and  Proposition 
2.5.7,  we  get  Jsp{g )  =  0.  If  g  is  a  singular  configuration,  then  there  exists  a  non¬ 


zero  vector  (  )  such  that  Jsp(g) 

UJQ  1 


(  ^  =0. 
\vo  J 


Therefore,  even  though  the  legs  of  the 


platform  are  locked,  it  can  still  move  with  velocity  (  J  in  directions  specified  by 

Vwo  J 

the  eigenvectors  corresponding  to  the  null  eigenvalue  of  Jsp{g)-  Prom  condition  (2)  of 
Proposition  2.5.7,  we  see  that,  geometrically,  those  directions  can  be  specified  by  the 
property  that  the  velocity  of  the  joint  Pi  is  either  perpendicular  to  the  i-th  leg  vector  S,t 
or  is  zero,  for  all  *  =  1, ...  6. 


Now,  for  some  a  ^  0  ,  suppose 


(i)‘ 


£ 


is  such  that  Jsp(g)  (  ^  )  =  Ed.  But  if  g  is  a 


singular  configuration  and  ("  ^  €  Afg(Jsp),  we  also  have  Jsp(g)  [  ^  ^  ^  ^ 1  - 


w/  +  \w0 


)]=»• 


Therefore,  a  determines  the  platform  motion  only  up  to  an  element  of  Afg(Jsp)- 


Definition  2.5.9  (Singular  Surfaces) 

The  loci  of  singular  configurations  in  configuration  space  are  called  singular  surfaces. 


From  Corollary  2.5.8,  there  exists  an  indeterminacy  in  the  motion  of  our  system, 
whenever  it  is  in  a  singular  configuration.  This  indeterminacy  is  undesirable  when  we  use 
the  platform  for  e.g.  repositioning  vision  sensors  and  perform  exploratory  visual  tasks. 
An  example  of  this  is  shown  in  section  2.5.2.  Moreover,  by  the  duality  of  kinematics 
and  dynamics,  there  exist  wrenches  that  the  platform  cannot  resist  while  at  singular 
configurations.  This  may  be  equally  undesirable  if  the  system  is  used  with  tactile  sensors 
to  perform  haptic  exploration.  On  the  other  hand,  limiting  the  motion  of  the  platform 
only  to  areas  of  the  configuration  space  which  are  free  of  singularities,  as  is  frequently 
done  in  practice,  is  not  possible  without  excessively  restricting  the  workspace  of  the 
manipulator.  Therefore,  we  should  either  try  to  avoid  singular  configurations  as  much 
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as  possible  or  just  drive  through  them,  in  such  a  way  that  the  dynamics  of  the  system 
resolves  the  indeterminacy  in  the  platform  motion. 

2.5.2  Planar  Parallel  Manipulators 

In  order  to  present  concrete  examples  of  our  ideas  about  planning  for  parallel  ma¬ 
nipulators,  we  chose  a  particular  planar  platform  architecture  (fig.  2.5.2),  which  is  simple 
enough  to  allow  analytic  derivation  of  the  forward  kinematic  map  and  easy  visualization 
of  the  singular  surfaces,  but  also  generic  enough  to  demonstrate  the  applicability  of  our 
results. 


Fig.  2.5.2:  Planar  Parallel  Manipulator 


We  maintain  the  notational  conventions  of  section  2.5.1,  with  the  necessary  adap- 

(R  T\  ( COsG  ~sm0  x\ 

tation  to  the  planar  case.  In  particular  g  =  (  )  =  sin  9  cos  9  y  ]  £  G  = 

' U  1  '  \  0  0  1/ 

SE( 2)  is  the  configuration  matrix  of  the  platform  with  respect  to  the  basis  coordi¬ 
nate  system  centered  at  Op,  where  9  is  the  angle  of  the  platform  with  the  horizon¬ 
tal  and  T  =  {x,y)T  is  the  position  of  the  center  Op  of  the  platform  coordinate  sys¬ 


tem.  Again  Pi  =  T  +  R  pPi,  where  5,  = 


def  (  Sn 


2 


=  Pi  -  Bi  =  T  +  R  PPi  -  B%  or 


ppt 

1 


Bi 


,  for  g  €  SE{ 2)  . 


In  the  case  of  fig.  2.5.2,  *PX  =  PP2  =  (-/3,  0)T,  PP3  =  ({3,  0)T,  Bx  = 
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(-a,  0  )T  ,  S2  =  (  —j  ,  0  )T,B3  =  (a,  0)T.Then, 

Si  ( Sn  ,  5i2  )T  =  {a  +  x  —  (3 cos  9,  y  —  (3  sin  0)r  , 

S2d=  (S2 1,  S22)T  =  +  x  -  (3cosd,y  - /3sind)T  ,  (2.5.9) 

S3  d=  (531 ,  532  )T  =  (~a  +  x  +  (3cos9,y  +  f3sin8)J  . 

Moreover, 

Mi  —  —aSi2  ,  M2  —  ——522  )  M3  -  q;532  ■  (2.5.10) 

The  inverse  kinematic  map  is  derived  directly  from  (2.5.3): 


(oi  (g)\  (  yffii  +  Sf2  \ 

<7(9)  =  I  Mg)  ]  =r~1(g)  =  (  v/5221  +  5222  )  (<?) 
W<?)/  \v/^IT+522/ 


(2.5.11) 


The  forward  kinematic  map ,  i.e.  the  platform  configuration  g  G  SE(2)  as  a  function 
of  the  link  lengths  Oi,  <72,03  is  obtained  analytically  if  we  observe  that  (x,y,Q)  can  be 
easily  determined  provided  the  position  of  the  end-points  of  the  platform  Pi  and  P3  is 
known.  But  point  Pi  lies  on  the  intersection  of  cycles  (Si,  cti)  and  (S2,  cr2)  and  point  S3 
lies  on  the  intersection  of  cycles  (Si,  2/?)  and  (S3, 0-3)  (fig.  2.5.3).  Thus,  this  problem  has 
in  the  generic  case  four  solutions,  while  in  special  cases  (which  correspond  to  singular 
configurations  of  the  platform)  it  may  have  one,  two  or  infinite  solutions. 

Let  £  (£i,£2)T  and  S  =f  (Hi,S2)t.  From  Definition  2.5.1,  adapted  to  the  case  of 

SE( 2),  we  get  the  spatial  and  body  velocities: 


Conversely, 


def  .  _ 1 

=  99 


/  0  —9  x  +  9y\ 
I  9  0  y  —  9x  J 

\0  0  0  / 


def  _  1  . 

=  9  9  = 


-8 

0 

0 


x  cos  9  +  y  sin  9 
— ±sin0  +  ycos9 
0 


9  —  u  =  Cl 


(2.5.12) 


x  =  £1  —  uy  =  cos#Hi  —  sin#S2 
y  —  £2  +  ux  =  sin  9E 1  +  cos  0S2  . 
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Fig.  2.5.3:  Forward  Kinematics  Solutions 
From  Theorem  2.5.3  we  have: 


=  Jsp(g)  ^6^  =Js{g) 


(2.5.13) 


/  oi  0  0  \  /  Sn  S\2  —aS\2  \ 

where  S(cr)  =  0  a2  0  ,  Jsp(g)  =  S2i  S22  -f  S22  and 

\  o  0  <73/  S32  OiS32  ) 

,  f  ((Sn  S12)R  0(8m0Su-<x*eS12)  \ 

Mg)  =  I  (S2i  S22)R  P(sm0S2i  —  cos 0S22)  . 

\(S31  S32)R  /3{-smdS3i  +  cos9S32)J 

From  Definition  2.5.6,  forward  kinematics  singularities  are  platform  configurations 
g  G  SE( 2)  where  Jsp  is  singular,  i.e.  where  detJSp(g)  =  0  .  From  (2.5.13)  we  have 
detJsp(g)  =  a/3(y  —  fd  sin  6)  [y  cos  9  —  (x  —  a)  sin#]  .  This  leads  to  two  kinds  of  singular 
configurations: 

1)  Those  where  X*  =  {(x,y,9)\y  =  /3sin# }  shown  in  the  generic  case  in  fig.  2.5.4a.  This 
condition  corresponds  to  configurations  where  the  leg  axes  intersect  at  point  B3.  The 
locus  of  those  configurations  in  (x,y,9)~ space  is  a  ruled  surface  shown  in  fig.  2.5.5. 

2)  Those  where  X*  =  {(x,  y,  9)\y  cos  9  —  (x  —  a)  sin#}  shown  in  the  generic  case  in  fig. 
2.5.4.b.  This  condition  corresponds  to  configurations  where  the  leg  axes  intersect  at 
point  Pi.  The  locus  of  those  configurations  in  (x,y,9)~ space  is  a  helicoid  shown  in  fig. 
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(a) 


(b) 


Fig.  2.5.4:  Kinematic  Singularities 


2.5.5. 


Thus,  the  singular  surfaces  for  the  manipulator  of  fig.  2.5.2  are  two  2-dimensional 
manifolds.  Their  intersection  is  composed  of  the  lines  {y  =  0,9  =  nn}  and  a  circular 
helix  with  radius  (3  and  axis  the  line  {x  =  a,y  =  0}  (see  fig.  2.5.5).  The  singular 
surfaces  divide  the  configurations  space  SE( 2)  of  the  planar  parallel  manipulator  in  four 
disjoint  parts  corresponding  to  y  >  /3sin#,  y  <  /?sin#,  ycos#  >  (x  -  a)  sin#  and 
ycosO  <  (x  —  a)  sin#. 

Inverse  kinematic  singularities  occur  when  E  is  singular,  i.e.  when  det(E)  =  0.  Then 
at  least  one  of  the  platform  legs  has  zero  length.  Observe  that  they  also  correspond  to 
forward  kinematic  singularities,  therefore  they  will  not  be  considered  separately. 


As  observed  in  Corollary  2.4.8,  at  singular  configurations  the  input  a  determines  the 
platform  motion  only  up  to  an  element  of  the  null  space  of  J$p  or  of  Jb-  In  the  case  of  sin¬ 
gular  configurations  of  the  first  kind,  where  A*  =  {(x,  y ,  9)\y  =  0 sin0,  sin#  ^  0},  we  can 
find  that  the  null  space  Mxt(J)  is  a  one-parameter  family  of  rotations  around  point  f?3. 


In  particular,  Nx,  (  Jsp ) 


In  the  case  of  singular  con¬ 


figurations  of  the  second  kind,  where  A*  =  {(x,  y,  9)\y  cos  9  =  (x—a)  sin#,  y  ^  (3  sin#,  x 
a, sin#  7^  0, cos#  7^  0}  we  can  find  that  the  null  space  Mx,(J)  is  a  one-parameter 


family  of  rotations  around  point  P\.  In  particular,  Mx,  (  J,sp  ) 
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(a)  (b) 

Fig.  2.5.5:  Singular  Surfaces  and  Singularity  Avoidance 


(x  -  a  —  j3  cos  0)  tan  6 ' 
—  (x  -  /3cos6) 

1 


w  >  or  Afx,  (Jb) 


Physically,  this  means  that  those  rotations  cannot  be  controlled  by  our  system  input 
a.  If  we  start  at  configuration  I  of  fig.  2.5.6  and  apply  input  {&]  =  b2  =  0,d3  ^  0}, 
the  platform  is  expected  to  rotate  around  point  P\.  From  the  input  alone  though,  the 
final  configuration  cannot  be  determined.  We  may  end  up  in  configuration  II  or  in 
configuration  III.  This  is  the  source  of  the  problems  discussed  at  the  end  of  the  previous 
section. 


Fig.  2.5.6:  Effect  of  Singularities 
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However,  by  properly  planning  the  system’s  trajectory,  it  is  often  possible  to  avoid 
singular  configurations.  An  example  is  shown  in  fig.  2.5.5.b.  This  will  be  further 
discussed  in  the  next  chapter. 
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CHAPTER  THREE 


MOTION  PLANNING  BASED  ON  PATH  CURVATURE 


3.1  Introduction 

In  this  chapter  we  consider  solving  analytically  some  instances  of  the  motion  planning 
problem  for  robotic  systems  with  holonomic  constraints.  The  motion  planning  problem 
that  we  consider  is  to  specify  a  trajectory  of  the  system  that  has  certain  desired  kinematic 
and  dynamic  properties  and  that  connects  a  given  initial  and  final  configuration.  Because 
of  the  holonomic  constraints,  the  configuration  space  of  the  robotic  systems  can  be 
considered  as  an  n-dimensional  Lie  group  G,  thus  its  trajectory  is  a  curve  g{.)  C  G,  with 
given  end-points  <y ( 0 )  and  g(T).  The  curve  g{.)  specifies  a  corresponding  curve  £(.)  on 
the  Lie  algebra  Q  of  G,  which,  through  identifying  Q  with  JRn  by  a  choice  of  coordinates, 
corresponds  to  a  curve  ct(.)  on  IRn.  There,  we  can  employ  differential  geometric  tools 
such  as  its  curvature  and  the  associated  Frenet-Serret  moving  frame  to  describe  a.  We 
will  consider  the  problem  of  finding  the  curve  that  optimizes  a  curvature-squared  cost 
functional  among  all  the  curves  on  G  that  connect  the  given  end-points.  This  can 
naturally  be  viewed  as  an  optimal  control  problem  involving  a  left-invariant  dynamical 
system  that  evolves  on  SE(n),  the  group  of  rigid  motions  on  lRn.  The  nonholonomic 
nature  of  this  system  allows  us  to  achieve  controllability  with  fewer  controls  than  the 
dimension  of  SE(n)  ((n  —  1)  controls,  while  the  dimension  of  SE(n)  is  |n(n  +  1)). 

The  motion  planning  problem  is  related  to  the  specification  of  a  path  in  configuration 
space,  so  that  certain  objectives  related  to  a  specific  task  are  optimally  met.  Planning 
in  the  configuration  space  of  the  manipulator  allows  us  to  solve  the  problem  without 
worrying  about  the  specifics  of  the  manipulator  architecture.  The  specification  of  the 
corresponding  joint  trajectories  is  done  through  the  inverse  kinematics,  which,  in  the 
case  of  parallel  manipulators,  is  easily  solvable. 

Suppose  we  are  interested  in  driving  our  system  from  an  initial  configuration  to  a 


42 


final  one  is  such  a  way  that  the  path  is  as  little  “curly”  as  possible.  We  do  not  want  to 
specify  further  the  shape  of  the  path  a-priori  (e.g.  by  choosing  a  straight  line  or  circular 
arc),  in  order  to  maintain  some  necessary  flexibility.  Such  a  situation  occurs  when  we 
try  to  move  from  one  side  of  a  singular  surface  of  a  parallel  manipulator  to  the  other 
(see  section  3.4  ).  A  “curly”  path  will  force  us  to  move  close  to  this  surface  for  a  long 
time,  or,  worse,  cross  it  multiple  times.  On  the  other  hand,  a  straight  path  between 
an  initial  and  a  final  configuration  may  not  allow  us  the  necessary  flexibility  to  cross  at 
some  “favorable”  point. 

This  situation  of  a  desired  path  which  is  as  little  “curly”  as  possible  is  very  similar  to 
the  classical  elastica  problem,  where  a  flexible  thin  rod  is  allowed  to  deform  while  its  ends 
are  fixed  (Jurdjevic  [a];  Jurdjevic  [b];  Jurdjevic  [1990];  Jurdjevic  [1992]).  Its  equilibrium 
configuration  is  known  to  correspond  to  the  extremals  of  a  variational  problem  with 
constraints,  where  a  cost  functional  involving  the  square  of  the  geodesic  curvature  of  the 
rod  is  minimized. 

We  consider  the  optimal  control  problem  of  minimizing  a  curvature-squared  cost 
functional.  This  is  similar  to  the  classical  problem  of  the  elastica.  We  obtain  analytical 
solutions  of  the  optimal  trajectories  and  apply  them  to  the  solution  of  the  singularity 
avoidance  problem  for  the  planar  parallel  manipulators  that  we  considered  in  chapter  2. 
The  solutions  to  the  problem  of  the  elastica  provide  us  with  trajectory  segments  which 
are  (locally)  optimal.  Those  can  be  patched  together  to  form  a  trajectory  that  links  the 
desired  end-points.  Because  we  have  a  family  of  optimal  segments  which  is  richer  than 
the  usual  straight-line  and  circular-arc  segments  most  motion  planners  consider,  we  can 
choose  those  segments  so  that  the  path  curvature  is  continuous,  even  at  the  joints  of 
the  segments.  This  is  important  in  various  applications  (e.g.  autonomous  vehicles  on  a 
manufacturing  floor  (Fleury  et  al.  [1993];  Kanayama  &  Hartman  [989  ];  Nelson  [1989a]; 
Nelson  [1989b])).  In  the  case  of  a  general  motion  of  the  manipulator,  the  configuration 
space  is  SE( 2),  while,  if  the  motion  is  restricted  to  a  2-dimensional  manifold,  it  can  be 
JR 2  or  JRxS1.  This  last  case  is  considered  in  section  3.2,  while  the  case  of  a  3-dimensional 
manifold  is  considered  in  section  3.3. 

3.2  Curvature— based  Planning  for  2— dimensional  Manifolds 

First  we  consider  the  case  of  a  translating  planar  parallel  manipulator.  The  configu- 
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ration  space  of  the  system  is  then  IR 2  with  coordinates  <j  =  (x,y).  A  path  in  configuration 
space  is  a  regular  curve  a(t)  =  (x(t),y(t))  in  IR2.  Alternatively,  consider  the  case  where 
the  planar  platform  translates  along  a  specific  line  and  also  rotates.  Then  the  configura¬ 
tion  space  is  IR  x  S1  .  If  we  consider  motion  on  a  chart  of  this  group,  the  problem  setup 
is  exactly  the  same  as  in  the  translational  case. 

Intuitively,  given  a  fixed  length  L  and  end-points  cr(0)  and  a(L)  in  IR2,  the  result  of  a 
planning  scheme  based  on  minimizing  the  curvature-squared  cost  functional  |  Jk2(s)  ds, 
will  be  a  curve  a (s)  similar  to  curve  (I)  in  fig.  3.2.1,  rather  than  similar  to  the  “curlier” 
curves  (II)  and  (III).  The  flexibility  of  this  approach  stems  from  our  ability  to  choose 
the  length  L  at  will,  thus  manipulating  the  shape  of  the  path. 

t  y 


X 


Fig.  3.2.1:  Configuration-Space  Paths 

Such  a  solution  to  the  planning  problem  only  affects  the  geometric  invariants  of 
the  path,  i.e.  it  specifies  its  shape,  but  not  the  velocity  with  which  this  is  traversed. 
This  extra  degree  of  freedom  can  be  used  to  optimize  dynamical  properties  of  the  system 
trajectory,  such  as  minimizing  acceleration  or  optimizing  the  power  requirements  of  the 
system  actuators.  Of  course,  in  the  case  when  the  magnitude  of  the  velocity  is  constant 
(a  non-trivial  case  since  the  direction  of  the  velocity  may  vary,  which  is  often  of  great 
interest,  as  in  the  case  of  autonomously  guided  vehicles),  we  know  that  the  curvature 
is  proportional  to  the  magnitude  of  the  acceleration,  therefore  a  minimal-curvature- 
squared  path  minimizes  acceleration  and,  thus,  the  current  to  the  motors  driving  the 
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platform  (Kuo  [1987]).  The  curvature  of  the  optimal  paths  is  bounded  and  its  bounds 
can  be  selected  by  the  designer.  In  addition,  the  curvature  varies  continuously,  thus  the 
acceleration  does  not  have  sudden  jumps.  Moreover,  the  jerk,  which  is  defined  as  the 
derivative  of  the  acceleration,  is  proportional  to  the  derivative  of  the  curvature,  which  can 
be  shown  to  be  bounded  on  minimal  curvature-squared  paths.  Therefore,  the  proposed 
planning  scheme  can  optimize  the  system  trajectory  with  respect  to  both  kinematic  and 
dynamic  criteria. 


3.2.1  Curvature-based  Planning  Criteria 

Let  er(s)  =  (x(s),y(s))  be  an  arc-length  parametrization  of  a  curve  a  in  JR2,  where 

t  t 

the  arclength  is  s(t)  =  J  ||^||2dT  =  J  \/x2(t)  +  y2{r)  dr ,  for  0  <  t  <  T  .  Then  <r(s) 

o  o 

is  a  unit  speed  curve  with  total  length  L  =  s(T).  After  properly  defining  its  tangent 
vector  Vi(s)  and  its  normal  vector  v2{s)  (Millman  &  Parker  [1977]),  we  define  the  (plane) 
curvature  of  the  curve  as  k(s)  —<  ^(s),v2(s)  >2  ,  where  < ,  >n  is  the  inner  product 
in  IRn.  We  are  interested  in  minimizing 


L 


If  6  is  the  angle  that  the  tangent  v\  makes  with  the  horizontal  (fig.  3.2.2),  we  have 
that  Vi (s)  =  and  ^(s)  =  ^  “$>)  •  Then  k{s)  =  6(s)  and  from  the 

Frenet-Serret  equations  we  get: 


^(s)  =  v i(s)  ,  =  k(s)u2(s)  ,  ~^j(s)  =  ~k(s)vi (s)  .  (3.2.1) 

/  \  /  cos  9  —  sin#  x\ 

Defining  g(s)  =  q1  Vq  1  J  ^  =  (  sin^  cos^  y  j  (s)  e  ^  ~  ^(2)  >  the  system 
(3.2.1)  can  be  viewed  as  a  left-invariant  system  on  the  Lie  group  G  =  SE( 2)  of  the  form: 
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Fig.  3.2.2:  Curve  in  the  Plane 


lw=9(s) 


-k(s) 

0 

0 


(3.2.2) 


=  s(s)(-42 +  =  X2(g{s))  +  n(s)Xi(g(s))  , 


where  the  left-invariant  vector  fields  Xi  are  defined  as  Xi(g(s))  =f  g(s)Az  and  where  the 
matrices  At  belong  to  the  following  basis  of  the  Lie  algebra  Q  of  G  : 


(  / 0  -1  0\  (0  0  iWo  0  0\\ 

{AUA2,A3}  =  l|l  0  0  ,  0  0  0  ,  0  Oi  l.  (3.2.3) 

l \0  0  0/  \0  0  0/  \0  0  0 J) 


For  Xl  a  left-invariant  vector  field  of  the  form  Xi(g)  =  gAi  ,  i  =  1,2,3, 
with  9  =  T)  and  A  =  ^  ^  ,  we  have  <fg(t)  =  getA'  = 

T  +  RAi  1  (etAl  -  I)(H  ^  _  Then  from  (2  3  4).  ^  ^.j  =  g{A.Aj  _  AjA%)  = 

g[Ai,Aj\  ,  where  [Ai,  Aj)  —  AlA]  —  AjA\  is  the  commutator  of  the  matrices  Al  and  Av 
It  is  easy  to  verify  the  following  results: 

[Ai,A2]  =  A3  ,  [^4i,v43]  =  —A2  ■ 
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Then,  for  the  Jacobi-Lie  brackets  of  the  vector  fields  Xi,  we  get  corresponding  results. 


Consider  now  the  following  variational  problem: 

Problem  P2  : 

Minimize  77  =  §  J^«2(s)ds 

on  the  dynamical  system  ^  =  g(A2  +  with  g  6  G  =  SE( 2), 

with  k  €  U  =  1R  and  with  given  boundary  conditions  <r(0)  and  a(L). 

This  problem  falls  in  the  framework  of  problems  studied  by  (Bryant  &  Griffiths  [1986]) 
and  (Jurdjevic  [1990]),  where  it  was  shown  that  the  curvature  of  the  optimal  paths 
satisfies  an  equation  of  the  form:  (jj)2  +  (Ci  -  \k2)2  =  C2  .  Subsequently  we  will  derive 
this  (Bryant-Griffiths)  equation  for  our  system  using  the  Maximum  Principle  formalism. 

3.2.2  Controllability 

Since  we  are  dealing  with  an  optimal  control  problem  with  boundary  conditions,  we 

should  examine  the  controllability  of  our  dynamical  system. 

The  group  G  =  SE{ 2)  is  non-compact  (therefore  the  controllability  results  of  e.g. 

(Jurdjevic  &  Sussmann  [1972])  do  not  apply  directly),  but  can  be  viewed  as  the  semi- 

direct  product  of  the  vector  space  V  =  M2  and  of  the  compact  connected  Lie  group 

K  —  50(2),  denoted  G  —  V  x$  K  ,  with  Lie  algebra  Q  =  IR 2  x  so(2)  . 

Consider  a  subset  T  of  Q  and  the  associated  family  Xr  of  left-invariant  vector  fields 

on  G.  Then  X  G  Xp  if  and  only  if  Xe  £  T  ,  where  Xe  is  the  value  of  X  at  the  identity  e 

of  G.  Moreover,  consider  the  semi-group  5(T)  generated  by  (J  {exp  At  :  *>0}. 

.4er 

Definition  3.2.1  The  family  Xr  of  left-invariant  vector  fields  is  transitive  on  G  if 
S(T)  =  G  . 


Notice  that  if  T  is  the  set  of  elements  of  Q  associated  with  a  left-invariant  control 
system  (T,  17),  where  U  is  a  class  of  admissible  controls,  then  we  know  that  for  every 
go  €  G,  there  exists  a  unique  solution  g  of  (r,  U)  corresponding  to  u  G  U  and  defined 
for  0  <  t  <  00  such  that  g( 0)  =  go  ■  Denote  this  solution  by  g(g0,u,t )  .  If  for  some 
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T  >  0  ,  g(g0,u,T)  =  gi  ,  we  say  that  the  control  u  steers  gQ  to  gi  in  T  units  of  time.  If 
there  exists  u  £  U  which  steers  g0  to  gx  in  T  units  of  time,  we  say  that  gx  is  reachable 
from  go  at  time  T.  We  denote  the  set  of  all  elements  gx  £  G  which  are  reachable  from 
go  at  time  T  by  lZ(g0,T)  and  the  set  of  all  elements  of  G  reachable  from  go  by  'R.(go)  ■ 
Then  TZ(go)  —  (J  ft(0o,T)  . 

0<T<oo 

Definition  3.2.2  The  system  (T,  U )  is  controllable  from  go,  if  R(go)  —  G  . 


Prom  the  left-invariance  of  (T,U),  it  follows  that  1Z(g0)  =  goR(e)  .  Therefore,  in 
order  to  study  controllability  of  (T,  U),  it  suffices  to  study  its  controllability  from  the 
identity. 

Definition  3.2.3  A  vector  v  E  V  is  a  fixed  point  under  K,  denoted  Kv  =  v,  if 
Kv  =f  {kv\k  £  K}  —  {u}  . 


Proposition  3.2.4  (Bonnard  et  al.  [1982]) 

Consider  a  group  G  —  V  x  5  K  ,  which  is  the  semi-direct  product  of  a  vector  space 
V  and  of  a  compact  connected  Lie  group  K.  Suppose  that  V  and  K  are  such  that  V 
admits  no  fixed  non-zero  points  with  respect  to  K.  Consider  a  subset  T  of  Q  and  the 
associated  family  X?  of  left-invariant  vector  fields  on  G.  Then  Xp  is  transitive  on  G  if 
and  only  if  the  Lie  algebra  £(T)  generated  by  the  elements  of  T  is  equal  to  Q. 


Lemma  3.2.5 

The  system  of  equations  (3.2.2)  is  controllable. 

Proof 

For  the  system  (3.2.2)  we  have  G  =  SE( 2)  ,  V  =  1R2  and  K  =  50(2)  .  The 

only  fixed  point  of  V  under  K  is  {0}  ,  since  it  is  the  only  point  ( x ,  y)  in  1R2  for  which 

cos#  —  sin  (A  f  x 
sin#  cos#  )  \y 
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The  system  (3.2.2)  can  be  written  in  the  form  ^  =  g{A2  +  kAi)  ,  for  k  £  IR  . 
Consider  T  =  {A2  +  kAi  \k  £  IR}  .  It  is  easy  to  see  that  £(T)  =  sp{Ai,A2,Az}  . 
Therefore,  £(T)  =  Q  and  from  Proposition  3.2.4,  we  conclude  that  the  family  of  vector 
fields  Xp  is  transitive  on  G. 

Observe  that  the  set  5(r)  defined  above  is  (by  definition)  the  set  of  all  elements  of 
G  reachable  from  the  identity  under  the  action  of  the  vector  fields  in  X r  .  Therefore, 
the  notion  of  transitivity  of  Xr  on  G  is  exactly  that  of  controllability  of  (P,  U)  from 
the  identity.  In  view  of  the  left-invariance  of  Xr ,  controllability  from  the  identity  is 
equivalent  to  controllability  from  any  g0  £  G  .  Thus,  the  system  (3.2.2)  is  controllable 
from  any  go  £  G  . 


3.2.3  Hamiltonian  functions,  Maximum  Principle  and  Poisson  Reduction 

Consider  a  left-invariant  control  system,  on  an  n-dimensional  matrix  Lie  group  G 
with  Lie  algebra  Q,  of  the  form 

m 

9  =  TeLg  •  =  g[Co  +  £>,(*)&)  ,  (3.2.4) 

i= 1 

where  g  £  G,  £*  £  Q  and  rq(t)  £  IR ,  such  that  {£o> £i>  •  •  •  >  Cm}  span  an  (m  +  1)- 
dimensional  subspace  h  of  Q,  where  dim  h  <  dim£.  Let  U  =  IR m  be  the  set  of  admissible 
controls. 

Consider  the  optimal  control  problem  of  minimizing 

pT  -j  pT  w 

T]=  L{u(t))dt  =  -  ^2  Jiuidt 

Jo  4  J 0  ,=1 

on  the  system  (3.2.4),  subject  to  the  boundary  conditions  ^(0)  =  g0  and  g(T)  =  g1. 
Assume  £  >  0  for  i  =  1, . . . ,  m.  Notice  that  L  does  not  depend  on  g  £  G. 

Let  <  ,  >  be  the  natural  pairing  between  elements  of  a  vector  space  V  and  those 
of  its  dual  space  V* .  Let  {Ai,A2,---,An}  be  a  basis  of  the  Lie  algebra  Q  and  let 
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{-4-1 ,  ^4-25  •  ■  ■  >Abn}  be  the  basis  of  its  dual  space  Q* ,  which  is  such  that  <  A[,Aj  >= 
where  is  the  Kronecker  symbol.  Any  g  £  G*  can  be  expressed  as  g  —  Ym=i  /A^i- 
The  tangent  and  cotangent  bundle  T G  and  T*  G  of  a  Lie  group  G  are  trivial  vector 
bundles,  therefore  can  be  represented  as  G  x  Q  and  G  x  Q*  respectively,  where  Q*  is  the 
dual  space  of  Q. 

A  generic  element  of  Q  =  se( 2)  is  represented  in  the  basis  {A\,A2,  A3}  as  A  = 


(A 

a)  -  ( 

oj-( 

—til  0,2  \ 
0  «3 
0  0  / 


,ai  6  1R  .  Consider  two  elements  A\ 


and 


A2  = 


of  Q  =  se( 2)  and  consider  the  following  inner  product  <  . ,  .  >g  on  G  : 


<  Ai,A2  >g=<  ai,a2  >n  +^tr(Aj A2)  , 

where  < ,  >„  is  the  inner  product  in  Mn  and  tr  is  the  trace  operator  in  lRnxn  .  A 
generic  element  g  oi  G*  can  be  identified,  using  the  Riesz  Representation  Theorem,  with 

an  element  g^  =f  ^  ^  of  G  via  the  pairing  g(A)  (also  denoted  as  <  g,  A  >),  with 

any  A  £  G,  defined  as: 

g(A)  =<  g,A>  =f<  /iB,v4  >g 

=<  (^  *0  )  ’  (  0  0)  >e=<  a»'a  >n  +\tr(A»A)  ■ 

Consider  an  element  ag  =f  (g,g)  of  T*G  ss  G  x  G*  with  g  6  G  and  g  €  G*  and 
define  the  pre-Hamiltonian: 

^  m  m 

H\  (otg,  u)  —A  L(u)-\-  <  CCg,  TeLg  -£u  A*=  —  A  —  ^  '  Iiui  T  i^-gi  ■  (&>  T  'y  '  ^  1 

i= 1  «=1 

(3.2.5) 

where  A  =  0  or  A  =  1  and  where  < ,  >  is  the  natural  pairing  between  the  elements  of 
TG  and  those  of  T*G. 

Proposition  3.2.6  (Maximum  Principle) 

Suppose  </*(.)  is  a  trajectory  of  (3.2.4)  that  corresponds  to  the  control  u*(.)  that 
minimizes  17.  This  trajectory  is  a  projection  of  an  integral  curve  o*(.)  =  (g* (.),//*(.)) 
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of  the  Hamiltonian  vector  field  -XtfA(aj,u*)  defined  on  the  entire  interval  [0, L\,  which 
satisfies: 

i)  If  A  =  0,  then  p*(s)  A  0,  for  some  s  G  [0,  L]  (a*  is  not  the  zero  section  of  T*G ). 

ii)  H\(a*(s),u*  (s))  >  H\(a*(s),u(s)),  for  any  u(.)  G  U  and  for  almost  all  s  G  [0,  L\. 

iii)  When  L  is  fixed,  H\(a*(s),u*(s))  is  constant  for  almost  all  s  G  [0, L\.  When  L  is  free 
to  vary,  H\(a*(s),u*(s))  =  0  for  almost  all  s  G  [0,  L\. 


For  A  =  0,  the  integral  curve  a*  (.)  is  called  an  exceptional  extremal  and  is  in¬ 
dependent  of  the  cost  functional.  For  A  =  1,  a*(.)  is  a  regular  extremal.  In  either 
case,  since  the  set  of  admissible  controls  U  is  unbounded,  condition  (ii)  above  implies 
^^(a!*(s),u*(s))  =  0  and  <  0,  for  almost  all  s  G  [0,L]. 

Proposition  3.2.7  (Krishnaprasad  [1993]) 

Consider  the  left-invariant  control  system  of  (3.2.4)  and  the  optimal  control  problem 
of  minimizing  rj  subject  to  the  boundary  conditions  g( 0)  =  g0  and  g(T)  =  gx. 

Consider  the  hamiltonian 


i=l  * 


(3.2.6) 


where  p  C  Q*  is  an  integral  curve  of  the  Hamiltonian  vector  field  on  the  Lie-Poisson 
manifold  Q*_  =  (Q* ,  {  ,  }_),  given  in  local  coordinates  by  (2.4.3)  with  the  bracket  { ,  }_ 
given  by  (2.4.5)  and  (2.4.6).  Then,  the  regular  extremals  of  the  problem  are  given  by 


Ui  — 


^  ^ 
h 


(3.2.7) 


In  order  to  apply  this  result  to  problem  (P2))  let  ux  =  k  and  consider  the  pre- 
hamiltonian  for  A  =  1  : 


+  ( Oig  ,  TeLg 


(A2  +  ni.4i))  , 


(3.2.8) 
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where  ag  £  T*G  =  G  x  Q* .  However, 


(*g,TeLg  ■  (M2  +  uiMi))  —  ({TeLg)*  ■  ag,A.2  +  xxiMi) 

—  (m>  -A.2  +  u\A\)  , 


(3.2.9) 


where  m  =  ( TeLg )*  ■  ag  £  Q* .  Then, 

H  —  —  —  Ui+  <  /X, M2  >  +XXi  <  M>  Ml  >  . 

Prom  the  Maximum  Principle,  we  have: 

dH  .  ,  , 

-7; —  =  —  1x1+  <  m>Mi  >=  0  =>•  1x1  =<  xx,  Ax  >=  Mi  ,  (3.2.10) 

OU\ 

then  we  can  specify  the  problem’s  hamiltonian,  which  is  a  function  H  £  C°°(T*G)  : 

H  =<  /x, -4.2  >  +2  ^  /b*4i  >2=  M2  +  2^1  ■  (3.2.11) 

Thus,  the  trajectory  g  6  G  corresponding  to  the  optimal  control  xx  is  a  projection  of  an 
integral  curve  ag  =  (g,  m)  of  the  hamiltonian  vector  field  Xu  on  T*G,  that  corresponds 
to  the  function  H  £  C°°(T*G).  However,  from  (3.2.11)  we  observe  that  H  is  G-invariant, 
thus  the  hamiltonian  vector  field  Xu  can  be  reduced  to  a  hamiltonian  vector  field  Xh 
on  Q*,  that  corresponds  to  the  function  h  £  C°°(Q*)  : 

h  =<  fi,  A2  >  +  2  <  M>  Ai  >2=  M2  +  2^1  •  (3.2.12) 


For  the  given  basis  of  Q  =  se(2)  we  have  from  (2.4.6): 


/  0  -M3  M2\ 

A-  =  M3  0  0 

\-M  2  0  0/ 


Corollary  3.2.8 

The  regular  extrema  of  problem  (P2)  are  given  by 

n  =<  m>Mi  >=  Mi  ) 


(3.2.13) 


(3.2.14) 
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where  [i  C  Q*  is  an  integral  curve  of  the  Hamiltonian  vector  field  Xh  on  Q*_  corresponding 
to  the  hamiltonian 


h  — <  /i,^4.2  >  +2  <  M>-4i  >2—  M2  +  -Mi 


(3.2.15) 


This  integral  curve  is  given  in  local  coordinates  of  Q*  by 


Mi  =  “M3  , 


M2  =  MiM3  , 


(3.2.16) 


M 3  =  -M1M2  • 


The  integral  curves  m  C  Q*  of  Xh  are  the  intersections  of  the  level  sets  of  the  hamiltonian 
h  with  the  symplectic  leaves  of  Q*_ .  The  level  sets  of  the  hamiltonian  h  are  the  parabolic 
cylinders  {/1  :  |m?  +  M2  =  h}.  The  Casimir  functions  of  the  Lie-Poisson  manifold  Q*_  are 
of  the  form  $  =  $(m2  +  M2)-  Their  level  sets  (symplectic  leaves  of  Q*_)  are  the  circular 
cylinders  {fj,  :  m|  +  M3  =  C2}  (fig-  3.2.3). 


Fig.  3.2.3:  Integral  Curves  of  the  Hamiltonian  v.f.  Xh  on  the  Lie-Poisson  Manifold  Q*_ 

Proof 

The  result  follows  from  Proposition  3.2.7,  using  (2.4.3)  and  (3.2.7). 
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By  differentiation,  it  is  easy  to  verify  that  ^Mi  +  M 2  and  m2  +  fj,2  are  constant  on  the 
integral  curves  (3.2.16): 


d  / 1  9  \  .  .  (a.z.io) 

^(^Ml  +  M2)  —  MlMl  +  M2  -  —  MlM3  +  MlM3  =  0 


.  (3.2U6) 

(3.2.16) 


dt 


(m!  +  M3)  —  2(/i2A*2  +  M3M3)  —  2(/ii/u2M3  —  M1M2M3)  =  0 


3.2.4  The  Bryant-GrifRths  Optimality  Constraint 

Theorem  3.2.9  (Bryant-GrifRths  Optimality  Constraint) 

The  curvature  «(.)  that  corresponds  to  regular  extremals  of  the  variational  problem 
(P2)  satisfies  the  following  optimality  constraint  when  L  is  fixed: 

(^M)2  +  (H-^2W)2=C2,  (3.2.17) 

where  C2  d=  (^(0))2  +  (tf  -  §k2(0))2  with  C  >  0  . 

When  L  is  free,  the  curvature  satisfies: 

(^M)2  +  j*4W=C2,  (3.2.18) 


where  C2  (^(O))2  4-  5«4(0)  with  C  >  0  . 

Proof 

From  (3.2.16),  by  differentiating 


Mi  =  M2  =  M1M2  =  Mi(#  -  2^1)  • 


Observe  that 


d  [1  .21  ■  -  (3.2.19)  1  2 

rfi  L2^iJ  =  A*lA*i  =  -  -MjMi 


By  integrating  we  get 


(Mi)2  +  (-H-  —  2^1^  ~~  ’ 


(3.2.19) 
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where  C  =f  sj (j\{ 0)  +  (H  —  |/tf( 0))2.  Then  (3.2.17)  follows  from  (3.2.14). 
When  L  is  free,  then  H  =  0  and  (3.2.18)  follows  from  (3.2.17). 


3.2.5  Solutions  of  the  Bryant-Griffiths  Optimality  Constraint 

The  solutions  of  the  Bryant-Griffiths  optimality  constraint  for  free  L  (equation 
(3.2.18))  are  the  special  case  for  H  =  0  of  the  solutions  of  the  corresponding  constraint 
for  fixed  L  (equation  (3.2.17))  (see  case  3  below).  Thus  we  examine  only  the  solutions 
of  (3.2.17)  in  detail. 


Theorem  3.2.10 

The  generic  solution  of  the  Bryant-Griffiths  optimality  constraint  (equation  (3.2.17)) 
involves  Jacobi  elliptic  functions.  Some  special  cases  involve  only  elementary  functions. 
The  solutions  of  equation  (3.2.17)  can  be  classified  as  follows,  when  C  ^  0  : 


/ 2  2 

1.  H  >  C  :  The  curvature  k  is  given  by  k  =  ±k\  dn(^s  +  <5,  k ),  where  k  =  ■ 

K\  =  \/2(H  +  C)  and  =  \J2(H  —  C).  The  constant  J  depends  on  the  initial  condition 
k(0).  The  solutions  and  the  corresponding  (k,  «)-plot  are  shown  in  fig.  3.2.4. 

2.  H  =  C  :  The  curvature  k  is  given  by  «  =  sech^s  +  <5),  where  =  2 \fC  and 
the  constant  S  depends  on  the  initial  condition  k(0).  The  solutions  and  the  corresponding 
(k,  «)-plot  are  shown  in  fig.  3.2.5. 


3.  —  C  <  H  <  C  :  The  curvature  k  is  given  by  k  —  k\  cn(  ^Kl  +-^—s  +  8,  k ),  where 
k  =  ,  K‘  K\  =  \J2{H  +  C)  and  «3  =  \/2(C  —  H).  The  constant  8  depends  on  the 

l/Kl2+K32 


initial  condition  k(0).  There  are  three  subcases,  namely  H  <  0,  H  =  0  and  H  >  0.  The 
solutions  and  the  corresponding  (k,  «)-plots  are  shown  in  fig.  3.2.6. 

4.  H  =  —C  :  The  curvature  k  is  given  by  k  =  0. 

5.  H  <  —C  :  There  are  no  real  solutions. 

When  C  =  0,  equation  (3.2.17)  admits  only  the  constant  real  solutions  k  =  ±V2H. 

The  (k, /i)-plots  in  figs.  3.2.4.b-3.2.6.b  are  the  projections  on  the  (fii,  ^-plane  of 
the  integral  curves  of  the  Hamiltonian  vector  field  X^.  As  mentioned  already,  those  are 
the  intersections  of  the  level  sets  of  the  hamiltonian  h  (the  parabolic  cylinder  in  fig.  3.2.3) 
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with  the  symplectic  leaves  of  Q*_  (the  circular  cylinder  in  fig.  3.2.3).  The  projection  of 
their  intersection,  for  various  relative  positions  of  the  two  ruled  surfaces,  produces  those 
(k,  «)-plots  and  gives  rise  to  the  above  classification  of  the  solutions. 


(a)  (b) 

Fig.  3.2.4:  Solutions  of  the  Bryant-Griffiths  Optimality  Constraint  when  H  >  C 


(a)  (b) 

Fig.  3.2.5:  Solutions  of  the  Bryant-Griffiths  Optimality  Constraint  when  H  =  C 
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(a)  (b) 

Fig.  3.2.6:  Solutions  of  the  Bryant-Griffiths  Optimality  Constraint  when  —C<H<C 

Proof 

Let  C  ^  0.  From  (3.2.17)  we  have: 

(^)2  —  C2  -  (i/  -  i«2)2  =  -i  [«2  -  2{H  +  O]  [k2  -  2(H  -  C)]  .  (3.2.20) 

Define  gi(«)  ^  [k2  —  2(H  +  C)]  \k2  —  2{H  —  C)] .  To  specify  its  roots,  let  K  ^  k2  and 

define  g2{IC)  =f  [K  —  2 (H  +  C)}{) C  —  2 (H  —  C)],  whose  roots  are  K\  =  2 (H  +  C)  and 
JCz  =  2 (H  —  C)  .  Observe  that  K\,Kz  €  1R  and  K. 3  <  K\.  In  order  to  solve  (3.2.20), 
it  remains  to  specify  the  roots  of  <?i(k).  The  real  roots  of  g\ (k)  specify  the  constant 
solutions  of  (3.2.17).  Consider  the  following  cases: 

1.  H  >  C 

In  this  case  0  <  K$  <  K\.  Define  =f  y/K[  and  K3  \/K, 3  and  observe  that 

0  <  K3  <  Ki.  Then,  the  roots  of  gi(n)  are  {«i,  — «i, «3,  — ^3}  and  from  (3.2.20) 

1  1 

{Ts)2  =  ~4(k  "  Ki)[k2  ~  ~  ^  ~  ^  '  (3'2-21) 

From  this  we  see  that  for  ^  to  be  real,  we  should  have  (— <  k  <  — «3)  or  («3  <  n  < 
«l)- 
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Let  K3  <  k  <  Ki  and  assume  /c(0)  =  k3.  Then  ^7  >  0  and  from  (3.2.21)  and  (2.2.26)  we 
have: 


dn 

ds 


(a)  =  («i  “  «2)(«2  -  «1) 


/c(a)  s 

f  dn  I  f 

J  —  k2)(k2  —  k?)  2  J 

K-3  0 


dt 


(3.2.22) 


1  \Z*q2  ~  «32v  _  g 

«i  v  K 3  ’  '2 

k(s)  =  K]_k'  nd(^s,  A:)  =  «i  dn(^s  +  K(k),  k)  , 
z  z 


where  nd  and  dn  are  the  Jacobi  elliptic  functions  with  modulus  k  =  — 5 — —  and 

complementary  modulus  k'  =  ^.  Also,  let  m  =f  k2.  The  solution  k(.)  has  period  T  = 

^-2 K(k)  =  iKJ\kK  where  K(k)  is  the  complete  elliptic  integral  of  the  first  kind.  This 
solution  is  shown  in  fig.  3.2.4  (for  Ki  =  1  and  k3  =  0.5). 

Now  assume  k(0)  =  «i .  Then  77  <  0  and  from  (3.2.21)  and  (2.2.27)  we  have: 


J_  d  -1  (^(f)  V'Kl2  ~  «32n  _  S 
K\  ^  «1  ’  «1  2 

=>  k(s )  =  K!  dn("^~)  k)  . 


(3.2.23) 


Let  — «!  <  k  <  —k3  and  assume  k( 0)  =  —  Then  j*  >  0  and  from  (3.2.21)  and 
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(2.2.27)  we  have: 


^(s)  =  ^(k?  -k2){k2  -kI) 

k(s)  s 

_  f  dn  If 

J  \/(k2  —  k2)(k2  —  K3)  2  J 


dt 


(3.2.24) 


1  -1  /-«(«)  \/«i2  -  k32x  s 

=4>  —  dn  ( - ,  - )  =  - 

«i  Kl  Ki  2 

=*>  k(s)  =  — «x  dn(~~,k)  . 


Now  assume  k(0)  =  — K3.  Then  77  <  0  and  from  (3.2.21)  and  (2.2.26)  we  have: 
t;(s)  =  iKi  ~  k2){k2  ~  *  1) 


«(«)  « 

/  _  1  f 

J  \/(/c?  —  k2)(k2  —  k|)  2  J 

-«3  0 


dt 


(3.2.25) 


^  1  nd~x ( ~K(S}  \/^i2  -  ^32x  _  s 

\  2 


«i 


«3 


«1 


k(s)  =  —  kxA/  nd(^s,  fc)  =  — «i  dn(^2-s  +  K(k),k)  . 

Li  Z 


,*1 


2.H  —  C 

In  this  case  0  =  /C3  <  /Ci .  Define  k x  =f  v^i  and  «3  =f  y^3  =  0  and  observe  that 
0  =  K3  <  Kx-  Then,  the  roots  of  <71  (k)  are  {«x,—  Kx,0  (double  root)}  and  from  (3.2.20) 
we  get 

(^)2  =  \k2(ki2-k2).  (3.2.26) 

Observe  that  77  is  real  only  when  —  k\  <  k  <  k\.  Prom  (3.2.26)  we  get: 

d/c  1  r~f  T 

*  =  ±2*^  -  K  '  <3'2'27> 


By  integrating  and  after  defining  S  =f  ln( - KP  2),  we  get  for  0  <  k  <  : 

«i  4-\/  —  Kq 


k(s)  =  Ki  sech(^s  ±  (5) 

z 


(3.2.28) 
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and  for  —  K\  <  k  <0  : 

k(s)  =  —Ki  sech(^s  ±  5)  .  (3.2.29) 

The  positive  solution  is  shown  in  fig.  3.2.5  (for  <5  =  0  and  H  —  1). 

3.  -C<H<C 

In  this  case  /C3  <  0  <  K,\  .  Define  ki  =f  \f)Cl  and  k3  d=f  yf—K, 3.  Observe  that  when 
H  <  0  we  have  0  <  <  k3,  when  H  —  0  we  have  0  <  ki  =  K3,  and  when  H  >  0  we 
have  0  <  k3  <  K\.  The  roots  of  3i(k)  are  {/si,— ki,*k3,— i«3}  and  from  (3.2.20): 

(S)2  =  ~  k2)(k2  +  Ki)  =  ~  *2)(k2  +  k2)  '  (3-2.30) 

Prom  this  we  see  that  for  77  to  be  real,  we  should  have  —  kj  <  k  <  «i.  We  distinguish 
the  cases  0  <  k  <  ki  and  —  <  k  <  0. 

Let  0  <  k  <  K\  and  assume  n(0)  =  0.  Then  77  >  0  and  from  (3.2.30)  and  (2.2.23) 
we  have: 


dn 

ds 


|\/(«i  -k2)(k2  +  k3) 


k(«)  s 

j"  dK  1  jr 

J  V(«1  -«2)(k2  +  «§)  2  J 


dt 


(3.2.31) 


_‘ld-.(j£±3(tW,,t)_f 

yOtf  +  K3  «i«3  2 

K(«)  =  K,  k'  sd(^S+As ,  fc)  =  Kl  c„( 


a  +  3K(A:) ,  jfe)  , 


where  the  modulus  is  ?  and  the  complementary  modulus  is  k!  =  Jtl —  . 

The  solution  k  has  period  T  =  —7= — =4K(k)  —  -4=^=. 
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Now  assume  that  k(0)  =  «i.  Then  ^7  <  0  and  from  (3.2.30)  and  (2.2.21)  we  have: 


dn 

ds 


(s)  = 


\\j («i  -  «2)(k2  +  «!) 

k(«)  s 

f  dK  If 

J  \/{^i  —  k2)(k2  +  ^3)  2  J 


dt 


1 


:  cn 


-1 


.T— J-  (-«*)  =  * 

\/k?  4-  K3  m  2 

k(s)  =  K\  cn(  K-s  ,  A:)  , 


(3.2.32) 


where  the  modulus  is  the  same  as  before. 

Let  —  «i  <  k  <  0  and  assume  k(0)  =  0.  Then  77  <  0  and  from  (3.2.30)  and  (2.2.23)  we 
have: 


«(*)  s 

f  dn  _  1  f  + 

J  V(«1  -k2)(«2  +  «i)  2  J 


_ I _ sd-i/vS  +  S-^  =  £ 

(  K1/S3  (  }  ’  }  2 

*(.)  =  -KlA,'sd(^3S .  *)  =  Kl  cnfiSpI 


s  +  lf(fc),  fc)  , 


(3.2.33) 


where  the  modulus  and  complementary  modulus  are  the  same  as  before. 
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Now  assume  that  /c(0)  =  — «i.  Then  ^  >  0  and  from  (3.2.30)  and  (2.2.21)  we  have: 


d/c 


ds  (s)  =  -«2)(K2  +  «i) 

«(«)  s 

f  _ d/c _  I  f 

J  \J (k2  —  k2)(k 2  +  K3)  2  J 


dt 


VKi  + 


cn 


-1 


/Co 


e*)-i 


«(«)  =  -*i  cn(  V//-1-+— s  ,  A:)  =  /ci  cn(^1<^~-^- 


s  +  2if(fc) ,  *:)  , 


(3.2.34) 


where  the  modulus  is  the  same  as  before. 


4  .H  =  -C 

In  this  case  JC3  <  K-i  =  0.  Define  /ci  d=f  \J—K  1  and  /C3  =f  and  observe  that 

0  =  «x  <  K3  .  Then,  the  roots  of  (/c)  are  {0  (double  root),  i/c3,  -i/c3}  and  from  (3.2.20) 


(^)2  =  -^/c2(/c2  -  /C3)  =  ~7«2(/c2  +  k\)  <  0  .  (3.2.35) 

ds  4  4 

From  this  we  see  that  the  only  real  solution  in  this  case  is  k(s)  =  0. 

5  .H  <-C 

In  this  case  /C3  <  K\  <  0.  Define  /ci  =f  \f—K. 1  and  /c3  =f  y/—JCs  and  observe  that 
0  <  /ci  <  /c3  .  Then,  the  roots  of  gi(/c)  are  {i/ci,  —  i/ci,?/c3,  —  i/c3}  and  from  (3.2.20) 


d/c  2  _  _!/  2 
W  4 


1C  i)(k2 


/C3)  =  -^(«2  +  «i)(«2  +  «i)  <  0  . 


(3.2.36) 


From  this  we  see  that  there  are  no  real  solutions  in  this  case. 


6.  (Constant  solutions) 

Consider  the  case  ^j(s)  =  0 .  The  real  roots  of  <?i(/c)  /c4  —  AHk?  +  4 (H2  —  C2) 


are  then: 


k(s) 


r±\/2  (H±C), 

0  (double  root) ,  ±2\/C, 
<  ±y/2 (H  +  C), 

0  (double  root) , 

.  No  real  solution, 


for  C  <  H] 
for  H  —  C; 
for  —  C  <  H  <  C; 
for  H  =  -C; 
for  H  <  —C] 
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Prom  the  real  roots  of  gi(«)  we  get  the  constant  solutions  of  (3.2.17),  which  correspond 
to  linear  segments  («(s)  =  0)  or  circular  arcs  ( k(s )  —  k, o  /  0)  . 

Observe  that  the  solutions  for  H  =  C  and  H  —  —C  are  limiting  cases  of  the  elliptic 
solutions.  In  the  first  case,  the  modulus  k  tends  to  1  as  H  — >  C  and  in  the  second  k 
tends  to  0  as  H  — >  —C  (c.f.  equations  (2.2.9)). 

Let  C  =  0.  Prom  (3.2.17)  we  have: 

=  1  2)2  <Q  (3.2.37) 

as  l 

This  leads  to  imaginary  except  when  k(s)  =  ±\/2H  . 


3.2.6  Integral  Curves 

Assume  that  the  optimal  curvature  k(s)  has  been  specified  from  the  Bryant-Griffiths 
optimality  constraint  of  the  previous  section  for  the  system  (3.2.1)  parametrized  by 
arclength  for  0  <  s  <  L  and  with  known  boundary  conditions  cx(0)  and  a(L),  i.e. 
z(0)  =  x0,  y( 0)  =  y0,  x{L)  =  x\  and  y(L)  =  yx. 

By  rearranging  (3.2.1)  we  get: 


(£)w-U> 


(3.2.38) 


Both  systems  are  of  the  form  i(s)  =  A(s)x(s),  where  x(.)  6  1R2,  with  A(s)  and  J  A(t)cIt 

3 

commuting.  Then,  defining  A(s)  =  J  k(t)cIt  ,  their  solution  is: 

o 


S 


k{t) 

0 


dr 


(  «n  (0)  A 

\V2l(0)  J 


f  cosA(s)  sinA(s)A  /  ^11(0)^ 
y  — sinA(s)  cosA(s)  )  \f2i(0)  )  ’ 


(3.2.39) 
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\V22  (S)J 


k(t) 

0 


dr 


(  «12(0)\ 
\  ^22 (0)  ) 


(  cosA(s)  sinA(s)  \  /wi2(0)\ 
\  —  sin  A(s)  cosA(s)  J  \v22(0)  ) 


Moreover,  since  k  =  ,  we  have: 


0(s)  =  0O  +  A(s) 


(3.2.40) 


(3.2.41) 


If  0(0)  =  0o  and  0(L)  =  0i  are  known,  then 


A (L)  =  0i  -  0O  . 


(3.2.42) 


By  integrating  (3.2.1)  we  get: 


3 

s(s)  =  ar(0)  +  /  cos 0(r)dr 
o 

3  3 

=  a:(O)  +  cos0o  J  cos  A  (r)  dr  —  sin0o  J  sin  A  (r)  dr, 


(3.2.43) 


sin  0(r)dT 


=  y(  0)  +  sin  0O 


dr  +  cos  0o 


3 

J  sin  A(t)  dr  . 
o 


(3.2.44) 


Define  A(s)  =f  f  cos  A(t)  dr  and  B(s)  =f  /  sinA(r)  dr  .  Then  from  (3.2.43)  and  (3.2.44) 
o  o 

and  after  taking  the  boundary  conditions  x(0)  =  xo,y(0)  =  yo,x{L)  =  x\  and  y(L)  =  yi 
into  account,  we  get: 


A(L)  cos  0O  —  B(L)  sin  0o  =  x-i  —  x0  , 
A(L)  sin  0O  +  B(L)  cos0o  =  yi  -  yo 


(3.2.45) 
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and  from  these: 


A2(L)  +  B2(L)  —  [a^  —  xq]2  +  [yi  —  y0 ]2  .  (3.2.46) 


Observe  that  for  a  periodic  solution  with  x\  =  xq  and  yi  =  y0,  we  have  A2(L)  +  B2(L)  — 
0.  Provided  A2(L)  +  B2(L)  >  0,  we  can  solve  (3.2.45)  and  (3.2.46)  for  cos0q  and  sin  0()  : 


C°Se°  =  A2(L)  +  B2{L)  [A(L^Xl  ~  a°)  +  ~  Vo)]  . 

i  (3.2.47) 

Sm6°  =  A2{L)'+B2{L)  \-A^Vl  ~  y°)  _  B(L)(xi  ~  xo)]  • 


If  L  is  given,  together  with  (aj0,j/o)  and  (xi,yi)  (elastica  problem),  then,  after 
we  specify  k  from  the  Bryant-GrifRths  equation  (3.2.17),  we  can  use  (3.2.47)  to  find  the 
necessary  0O  and  (3.2.41),  (3.2.43)  and  (3.2.44)  to  specify  the  corresponding  trajectory 
g  of  our  system. 

If  L  is  free,  but  0(0)  =  0q  and  0(L)  =  9\  are  given,  in  addition  to  (xo,  yo)  and  (aq,  yi) 
(free  elastica  problem),  then,  after  we  specify  n  from  the  Bryant-Griffiths  equation 
(3.2.18),  we  can  use  (3.2.45)  or  (3.2.42)  and  (3.2.46)  to  specify  L  and  the  constant  C 
of  k.  Then,  from  (3.2.43)  and  (3.2.44)  we  specify  the  corresponding  trajectory  g  of  our 
system. 

We  now  apply  the  above  integration  procedure  to  the  various  solutions  of  the 
Bryant-Griffiths  equation. 

1.  H  >C 

Consider  k(s)  =  «idn(^1,A:)  ,  with  k  —  ^KlK~K3  -.  Then  from  (2.2.14)  A(s)  = 
2sin~1(sn(^,  k)),  for  s  €  ■  Moreover,  from  (2.2.21),  (2.2.8),  (2.2.10)  and 
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(2.2.14)  we  have: 


S 

.A(s)  =  J  cos  A (r)dr  = 


/ 

0 

s 

J 


1  ~2sn  2(~^,k) 


dr 


dr 


=  5  +  ^Nam(^}’^-¥. 

=  i1  ~  ^)S  +  ^2E(am^)’  k)  ’ 


7?(s)  =  J  sin  A(t) dr  =  J  2 sin l  —  sin2  dr 
o  o 

=  J  2sn(^,A:)  cn(^,A;)ofT 

o 

for  s  G  [-f ,  f  ]  .  From  this  and  (3.2.41),  (3.2.43),  (3.2.44)  and  (3.2.47),  we  get  the  system 
trajectory  and  90.  In  fig.  3.2.7,  the  curve  ( x(s),y(s ))  is  shown  (for  K\  =  1  and  k3  =  0.5). 

2.  H  =  C 

Consider  k(s)  =  k i  sech^s  +  <5).  Then  A(s)  =  4[tan_1(e^s+<5)  -  tan_1(e'5)]  and 


S 

A(s)  —  J  cos  A{r)dr  = 


:S  + 


8 


(1  +  72)2  «i(l  +  y2)2  [  1  +  72eKl®  1+72J  ’ 


Ve2s+S  V  +  £ 


S 

B(s)  =  J  sinA(r)dr  = 


V 


7s  + 


g72e^  _  p  _  £^  -y 
7(1 +72)2  '  «i7(1  +  72)2[  l+72eKlS  1  +  72  J 

U 

where  7  =  es  ,  V  =  472(1  —  72)  and  £  =  1  -  67s  +  74  .  When  8  =  0  : 


4  K,  4 

A(s)  =  —  s  H - tanh(— s)  and  B(s)  =  — 

Kl  2  K\ 


1  —  sech(^s) 


66 


Fig.  3.2.7:  Integral  Curves  for  H  >  C 


Fig.  3.2.8:  Integral  Curves  for  H  —  C 


From  those  and  equations  (3.2.41),  (3.2.43),  (3.2.44)  and  (3.2.47),  we  get  the  system 
trajectory  and  80  .  The  curve  ( x(s),y(s ))  is  shown  in  fig.  3.2.8  (for  h  =  1  and  5  =  0). 

3.  —  C  <  H  <  C 

Consider  k(s)  =  K\  cn(v  \  —s ,  k),  with  k  =  ,  K2X  2  .  Then  from  (2.2.14)  we  have: 

'  y/Ki+K3 

A (s)  =  2sm~1(ksn(^^^-s,k)),  for  s  G  [—  j,  j]  .  Moreover,  from  (2.2.10)  and  (2.2.14) 
we  have: 


s 


0 


COS 


dr 


=  —s  + 


VKi  +  4 


E(  am(^ 


+ 
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for  s  e  [—  |-,j]  •  Prom  those  and  equations  (3.2.41),  (3.2.43),  (3.2.44)  and  (3.2.47), 
we  get  the  system  trajectory  and  80.  In  fig.  3.2.9,  the  curve  (x(s),y(s))  is  shown  for 
various  values  of  k,\  and  (notice  that  the  symbol  m  in  those  figures  is  defined  as 


(a)  (b) 

6.  (Constant  solutions) 

Let  k{s)  =  0.  Then  A (s)  =  0.  From  (3.2.41),  (3.2.43)  and  (3.2.44): 

0(s)  =  90  , 

x(s)  =  Xo  +  COS  00 S  , 

y(s)  =  yo  +  sin0os  , 
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Trajectory  for  k_b  (s) 7k3=0 . 35,m=0 . 9 , -20<s<20 


(c) 

Fig.  3.2.9:  Integral  Curves  for  —  C  <  H  <  C 
for  all  s  £  [0,1].  Moreover,  from  (3.2.47)  for  A(s)  =  s  and  B(s)  =  0,  we  get: 


x\  —  xo  .  „  y i  —  yo 
cos  0o  =  — f — -  ,  sin  0o  —  — r — 


This  is  a  straight  line  in  1R2. 

Let  k(s)  =  k0  7^  0.  Then  A(s)  =  k0s.  From  (3.2.41),  (3.2.43)  and  (3.2.44): 

6(s)  =  60  +  k0 s  , 

1  .  1 

x(s)  =  xq  H - cos 0o sin(«os) - sin0o[l  —  cos(«os)]  , 

Ko  K  0 

1  1 

y(s )  =  y0  H - sin 0O  sin(K0s)  H - cos  0O[1  —  cos(«0s)l  , 

K0  Ko 

for  all  s  G  [0,  L\.  Moreover,  from  (3.2.47)  for  A(s)  =  ^  sin(K0s)  and  B(s)  =  ^[1 


cos(kos)],  we  get:: 


cos  0o  = 
sin  0o  = 


Ko 


2(l  —  cos(«o 

K  o 


-TjTyj  [(xi  -  X0)  s'ui(koL)  +  (yi  -  y0){  1  -  cos(k0T))] 


°  =  2(1  -  cos(kqL))  K*1  ~  xo)(cos(kol)  -  1)  +  {yi  ~  Vo)  sin(K0L)]  . 
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This  is  a  circular  arc  in  IB?  with  center  (xn  — —  sin0o,  Vo  +  —  cos  On )  and  radius  —  that 
starts  at  the  point  (£o,Vo)  and  ends  at  the  point  (£i,yi). 

3.3  Curvature-based  Planning  for  3-dimensional  Manifolds 


3.3.1  Curvature-based  Planning 

Consider  now  the  case  when  the  planar  platform  translates  and  rotates  or  when 
the  spatial  platform  translates  in  IB? .  The  configuration  space  is  then  a  3-dimensional 
manifold,  in  particular  SE( 2)  in  the  first  case  and  IB3  in  the  second.  If  we  consider 
motion  on  a  chart  of  SE( 2),  the  problem  reduces  to  IB 3  in  both  cases. 

Let  ct(s)  =  (x(s),y(s),z(s))  be  a  curve  in  IR3  parametrized  by  arc-length  and 
with  total  length  L.  The  tangent  vector  field  to  a  is  defined  as  wi(s)  =  ^j(s).  The 
curvature  of  a  is  defined  as  k(s)  =  ||ui(s)||3,  where  ||.||„  is  the  Euclidean  norm  in  IBn. 

The  principal  normal  vector  field  to  a  is  defined  as  v2  (s)  = 

vector  field  to  a  is  defined  as  v3(s)  =  ui(s)  x  v2  (s).  The  torsion  of  a  is  defined  as 
t(s)  —  —  <  v3(s),v2(s)  >3,  where  <,>n  is  the  inner  product  in  Mn.  The  Frenet-Serret 
apparatus  for  the  unit  speed  curve  a  is  («(s),t(s),ui(s),i;2(s),V3(s)}.  From  the  above 
definitions  and  the  Frenet-Serret  equations  we  get: 


_  yil*)  The  binormal 

K(S) 


Ys (*)  =  VM > 

^(S)  =  K(a)«2(a)  , 

^(S)  -  -k(s)vi(s)  +  t(s)v3(s)  , 
~{s)  =  -t(s)v2(s)  . 


(3.3.1) 


Defining  *0  *0  *1  )  €  C  =  SE(  3),  the  system  (3.3.1)  can 
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be  viewed  as  a  left-invariant  system  on  the  Lie  group  G  =  SE( 3)  of  the  form: 


tsM  =  s(s} 


0 

k(s) 

0 

\  0 


— k(s)  0  1\ 

0  — r(s)  0 

t(s)  0  0 

0  0  0/ 


=  s(s)(44  +  «(s)43  +  t(s)4i) 

=  Xi(g(s))  +  K,(s)X3(g{s))  +  t(s)X1  (g(s))  , 


(3.3.2) 


where  the  left -invariant  vector  field  Xi(g(s))  =f  g(s)At  and  where  the  matrix  At  belongs 
to  the  set 


{4i,  A2,  A3  ,4s,  Aq}  — 


/° 

0 

0 

°\ 

/ 

0 

0 

1 

0\ 

(° 

-1 

0 

°\ 

0 

0 

-1 

0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

1 

0 

0 

J 

-1 

0 

0 

0 

5 

0 

0 

0 

0 

5 

Vo 

0 

0 

0/ 

{ 

0 

0 

0 

0/ 

\0 

0 

0 

0/ 

0 

0 

°\ 

(Q 

0 

0 

°\ 

(° 

0 

0 

0 

0 

0 

0  ' 

f  0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

1  j 

J 

1 0 

0 

0 

0  ’ 

0 

0 

0 

Vo 

0 

0 

0 J 

0 

0 

0/ 

Vo 

0 

0 

As  we  see  from  the  following  Lie  bracket  relationships,  this  set  forms  a 


basis  for  tn6^L^ 


Algebra  Q  =  se(3). 


[^4.1 , ^4.2]  =  4.3,  [4i,4.3]  =  —42,  [4i,44]  =  0,  [4i,45]  =  46,  [4i,4g]  =  —4s, 

[42.43]  =  4i,  [42,44]  =  —4s,  [42,4s]  =  0,  [42, 46 ]  =  44, 

[43.44]  =  45,  [43,45]  =  —44,  [43, 4g]  =  0, 

[44.45]  =  [44,46]  —  [45,4s]  =  0. 


Similar  results  hold  for  the  left-invariant  vector  fields  Xi  under  the  Jacobi-Lie  bracket 
operation. 

Consider  now  the  following  variational  problem: 


Problem  P3  : 

Minimize 


L 


J  K2(s)ds  . 

0 


(3.3.4) 
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on  the  dynamical  system  =  g(s)(^4.4  +  k(s)»43  +  r(s)^4!)  with  g  €  G  =  SE(3), 
and  with  («,  t)  €  U  =  iR+  x  1R  and  with  given  boundary  conditions  cr(0)  and  a(L). 

This  problem  falls  in  the  framework  of  problems  studied  by  (Griffiths  [1983];  Langer  & 
Singer  [1984])  and  (Jurdjevic  [1990]),  where  it  was  shown  that  the  curvature  and  torsion 
of  the  optimal  paths  satisfy:  k2t  =  C\  and  +  k3  —  2C2k  —  2t2k  =  0  ,  where  C\  and 
C2  are  constants. 

3.3.2  The  Optimality  Constraint 

Theorem  3.3.1  (Griffiths  [1983];  Jurdjevic  [1990]) 

The  curvature  k  and  torsion  r  corresponding  to  the  regular  extremals  of  the  vari¬ 
ational  problem  of  minimizing  ^  J^K2(s)ds  on  the  system  (3.3.1)  satisfy  the  optimality 
conditions: 

d2K 

k2t  =  C\  and  2^-y  +  k3  -  2C2k  -  2r 2k  =  0  ,  (3.3.5) 

where  C\  and  C2  are  constants. 


3.3.3  Solutions  of  the  Optimality  Constraint 

Combining  equations  (3.3.5),  we  get: 

2^+«3-2C2«-2^|  =  0. 

dsz 

Multiplying  by  and  integrating,  we  get: 

(^■)2  +  4^4  ~C<2k2  +  ^2  =  >  (3.3.6) 

where  C3  d=  k2  +  |k04  -  C2k02  +  %q- 
Let  u  k2.  Then: 

A2  +  u3  -  4 C2u2  -  4 C3u  +  4 C\  =  0  ,  (3.3.7) 

ds 

which  is  solvable  by  elliptic  functions. 
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Let  Pi{u)  —  u3  —  4C2u2  —  4 C3u  4-  4C\.  This  cubic  will  have  in  general  three  roots, 
which  will  be  denoted  ui,u2,u3. 

By  the  substitution  yd=u—  ^C2,  we  bring  Pi(u)  to  the  reduced  form 

Pi{y)  =f  y3  +py  +  q  , 

where  p  —  — 1(3(?3  +  4 C2)  and  q  =f  ^[—32 Cf  —  36C2C3  +  27 C2]  .  Let  2/1 , 2/2, 2/3  be  the 
roots  of  P2(y). 

Consider  the  discriminant  D  d=  (|)3  +  (|)2  of  P2(y).  If  D  >  0,  then  P2(y)  has  one 
real  root  yi  and  two  complex  conjugate  ones  y2  and  y3,  while  if  D  <  0,  P2(y)  has  three 
real  roots  y\ ,  y2  and  2/3  (may  not  be  distinct) . 

Let  w  =  (— |  +  VD)s  and  v  *=f  (— |  —  VD) 3.  The  roots  of  P2(y)  are  then  : 


yi  =  w  +  v 

1 /  \  .V5,  ^ 

yi  =  ~2^w  +  v>  +*  —  (w-v) 

1  /  ,  ,\/3 .  . 

ya  =  +  v)  -  i  — (iu  -  v) 

and  the  roots  of  Pi  (u)  are 

4 

uj=%+3C2  ,  for;  =  1,2,3. 

Observe  that  u\  is  always  real. 

The  generic  solution  of  (3.3.7)  involves  elliptic  functions.  Consider  the  case  D  <  0  : 

In  this  case  all  roots  of  Pi(u)  are  real.  Observe  that  if  C2  <  0  ,  then  C3  >  0  .  Then, 
by  Descartes’  rule  of  signs,  Pi{u)  should  have  two  positive  and  one  negative  root.  Let 
—ui  <  0  <  u2  <  U3  be  those  roots  (this  is  a  permutation,  with  possible  sign  change  of 
the  roots  we  defined  earlier).  Then:  Pi  (it)  =  [u  +  ui)(u  -  u2)(u  -  u3)  and  from  (3.3.7): 
(fl)2  =  =  (u  +  ui)(u  —  u2){u3  —  u)  .In  order  for  ^  to  be  real,  we  should 

have  Pi(u)  <  0  whenever  u  is  not  constant.  Thus,  we  either  have  u  <  —ui  <  0  or 
0  <  u2  <  u  <  u3.  In  the  first  case,  since  k2  =  u,  we  will  not  get  real  k.  We  only  consider 
then  the  case  u2  <  u  <  u3  ,  when: 
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=  ±a/(«  +  m)(u  -  u2)(u3  -  u )  , 


(3.3.8) 


Let  u(0)  =  uz  and  <  0.  Prom  (3.3.8)  we  have: 


=  -y/(u  +  ui)(u  -  u2){u3  -  u) 
as 


u{s) 

-/: 


du 


y/(u  +  Ui)(u-  U2)(u3  -  U) 
u3  0 


-rJ 


ds 


(Gradstein  Ryzhik  [1980]) 


sj  113  +  111 


sn 


-l 


^3 


U 


u3  -  u2 


,k)  = 


u(s)  =u3-  («3 


.  2{Vu3  +  U1  ,  \ 

u2)sn  ( - - - s,k) 


=  «3  [l  - 
=  «3  [1  - 


Us__U2sn2(V»3_tlllSjJ:)] 
k2  2  1 

^2  sn  (rs>  fc)J  1 


where  k  =  ,  w2  =  ^  and  r  =  . 

The  solutions  of  (3.3.5)  are  then  k(s)  =  ±->/u(s)  and  t(s)  — 


(3.3.9) 


3.4  Singular  Configuration  Avoidance  for  Parallel  Manipulators 

Consider  the  planar  parallel  manipulator  examined  in  section  2.5.  The  kinematic 
singularities  of  the  system  divide  the  configuration  space  into  four  areas  (fig.  2.5.5).  We 
saw  that  for  several  grasping  and  sensor  reorientation  tasks  it  is  beneficial  to  optimize 
the  crossing  of  the  singular  surfaces.  From  fig.  2.5.5  it  can  be  seen  that  we  may  have 
to  cross  them  at  most  twice  for  every  choice  of  initial  and  final  configuration.  In  case 
the  crossing  of  the  singular  surfaces  cannot  be  avoided,  the  most  favorable  point  for  that 
has  to  be  specified,  since,  if  we  only  drive  through  the  singular  surfaces,  the  dynamics  of 
the  system  will  determine  the  platform  configuration  after  crossing  the  singular  surface. 
Therefore,  we  should  either  try  to  avoid  singular  configurations  as  much  as  possible 
or  just  drive  through  them,  in  such  a  way  that  the  dynamics  of  the  system  resolves 
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the  indeterminacy  in  the  platform  motion.  A  planning  scheme  not  restricted  to  the 
traditional  linear  configuration-space  trajectories  can  be  very  important  in  this  respect. 
If  we  consider  only  linear  configuration-space  trajectories,  our  system  may  have  to  cross 
unnecessarily  the  singular  surfaces  or  follow  paths  that  are  longer  than  needed. 

Example  3.4.1 

An  example  of  this  is  the  case  when  the  platform  translates  and  rotates,  keeping  the 
x-coordinate  constant  (say  x  —  xq).  The  corresponding  singular  surfaces  are:  X^  — 
{(x,y,9)\ y  =  (3sm9 ,  x  =  rE0}  and  A'*(2)  =  {(x,y,9)\y  cos#  =  (x0  -  a)sin0o  ,  x  =  x0}  . 
Those  singular  surfaces  are  shown  in  fig.  3.4.1  for  the  case  x0  -  a  >  (3.  If  we  consider 
moving  from  a  configuration  go  to  gi  along  the  linear  trajectories  I  or  II,  we’ll  have  to 
cross  unnecessarily  the  singular  surface  X^  twice  (fig.  3.4.3  and  3.4.4).  Trajectory  III  is 
better  in  this  respect,  but  is  also  undesirable,  since  it  is  not  smooth  and  it  is  much  longer 
than  necessary  (fig.  3.4.2).  It  is  then  beneficial  to  have  a  family  of  configuration-space 
paths,  which  on  one  hand  is  richer  than  the  set  of  straight  line  segments,  but  on  the 
other  retains  some  of  the  fundamental  properties  of  the  straight  lines. 


Path  1  (no  singularities) 


Fig.  3.4.1:  Singular  Surfaces 


Fig.  3.4.2:  Trajectory  III 


We  can  use  the  results  of  section  3.2,  in  order  to  specify  analytically  a  system 
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Path  3  (linear,  singularities  encountered) 


Path  2  (singularities  encountered) 


Fig.  3.4.3:  Trajectory  I  Fig.  3.4.4:  Trajectory  II 

trajectory  g(.)  that  avoids  singular  configurations.  Assume  that  we  are  given  go  G  SE( 2) 
and  <?i  G  SE(2),  i.e.  the  positions  and  tangent  vectors  at  the  end-points  of  the  path.  In 
this  case,  the  length  L  of  the  path  is  free,  thus  H  =  0,  k,\  =  k3  =  \J2C  and  k(s,C)  = 

«i  cn(^y^s, k),  with  k  =  y/*2+K2  ~  Then  A(s,C)  =  2sin_1  (^~  sn(\/Cs,  k)) . 

Moreover,  A(s,C)  =  —s  +  E[a,m(VCs),k )  and  B[s,C)  =  ^[l  -  cn (y/Cs,k)]  . 

We  need  to  specify  C  and  L  from  the  end-point  data.  From  the  boundary  conditions 
(3.2.42)  and  (3.2.46),  i.e.  from  A (L,C)  =  0X  -  90  and  from  A2{L)  +  B2{L)  =  (Xl  - 
xo)2  +  (yi  ~  yo)2,  we  get: 


c_  et2{M)  +  p2{M) 

{xi  -  s0)2  +  (yi  -  y0)2 

and 

r  i  _x(i  .  ,0i-0o,  A  i  „(  .  _wi  .  ,0i-0o„  A 

L  =  (ksm(-2->-V  =  jgF(sm  <*sm(^-»'*)  ’ 

where  a  and  (3  are  known  functions  of  A0  =  0X  —  0O.  The  last  expression  is  valid  for 
0  <  ^  sin(^-~g°  )  <  1,  i.e.  for  0  <  A0  <  ^  or  <  A0  <  27 r.  Outside  those  segments, 
we  need  to  split  the  path  in  several  segments  and  repeat  this  process  for  each  segment. 
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The  curvature  at  the  joints  of  those  segments  should  be  continuous.  If  we  define 


7(0)  =f  sn  1  Qsin(^)»fc)  =  F ^ sin  1  sin(^y^)),  , 

we  have: 

a(0)  —  —^(6)  +  2E[am('y(9)),k)  and  0(6)  =  1  —  01(7(0), k)  . 

Then  we  know  A(s),  A(s)  and  B(s),  thus  the  system  trajectory  g(s)  can  be  specified  by 
(3.2.41),  (3.2.43)  and  (3.2.44)  (fig.  3.4.5). 


(a) 


(b) 


Fig.  3.4.5:  Curvature-Minimizing  Trajectory 
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CHAPTER  FOUR 


G-SNAKES: 

NONHOLONOMIC  KINEMATIC  CHAINS 
ON  LIE  GROUPS 


4.1  Introduction 

In  this  chapter  we  are  interested  in  groups  with  a  real  finite-dimensional  non-abelian 
Lie  algebra  Q  (of  dimension  n)  and  £-node  kinematic  chains  evolving  on  them  under  a 
certain  class  of  nonholonomic  constraints,  where  the  constraints  force  the  velocities  of 
the  system  to  lie  in  a  subspace  of  Q,  which  is  not  a  subalgebra  of  Q  but  which  generates 
the  whole  algebra  Q  under  Lie  bracketing.  We  refer  to  systems  of  this  type  as  G-Snakes 
and  observe  that  they  possess  an  interesting  geometric  structure:  When  t  —  n  and  the 
codimension  of  the  constraints  is  one,  the  configuration  and  shape  spaces  of  the  system 
specify  a  principal  fiber  bundle  and  the  nonholonomic  constraints  determine  a  (partial) 
connection  on  it,  at  least  away  from  certain  configurations  which  we  call  nonholonomic 
singularities  (higher  codimension  cases  will  be  treated  briefly  in  section  5.4). 

In  section  4.2,  we  consider  the  kinematics  of  an  £-node  kinematic  chain  evolving 
on  an  n-dimensional  Lie  group.  The  Wei-Norman  representation  of  G ,  which  expresses 
an  element  of  the  group  as  a  product  of  the  one-parameter  subgroups  of  G,  and  the 
adjoint  action  of  G  on  Q  allow  us  to  express  in  a  compact  form  how  the  motion  of  each 
node  of  the  kinematic  chain  relates  to  that  of  the  other  nodes  and  to  the  global  motion 
of  the  system  and  how  this  latter  becomes  a  function  of  just  the  shape  and  the  shape 
controls  because  of  the  nonholonomic  constraints.  We  show  that  the  configuration  and 
shape  spaces  of  the  G -snake  specify  a  principal  fiber  bundle  and  that  the  nonholonomic 
constraints  determine  a  connection  on  it. 

In  section  4.3,  we  focus  on  3-node  G-Snakes  evolving  on  3-dimensional  Lie  groups 
(£  —  n  =  3).  In  particular,  we  examine,  apart  from  SE( 2),  the  Heisenberg  group  H( 3), 
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the  Special  Orthogonal  group  SO( 3)  and  the  Special  Linear  group  SL( 2).  We  derive  the 
corresponding  Wei-Norman  representation,  the  system  kinematics,  the  connection  and 
the  nonholonomic  singularities  in  each  case. 

For  reasons  having  to  do  with  ease  of  exposition,  we  limit  ourselves  to  matrix  Lie 
groups.  Extensions  to  arbitrary  Lie  groups  are  easy. 

4.2  Nonholonomic  Kinematic  Chains  on  Lie  Groups 

In  section  4.2.1  we  derive  the  kinematics  of  the  £-node  kinematic  chain.  In  section 

4.2.2  we  examine  the  geometric  structure  of  the  chain  kinematics  when  nonholonomic 
constraints  are  present,  using  the  theory  of  connections  on  principal  fiber  bundles. 

4.2.1  The  ^-node  Kinematic  Chain 

We  consider  an  A-node  dynamical  system  that  evolves  on  the  Cartesian  product 
Q  =  G  x  ■  ■  ■  x  G .  Its  trajectory  is  a  curve  g(.)  =  (.), . . . ,  g „(.))  C  Q.  On  each  copy  of 

V  ^  1 

l  times 

G,  the  system  traces  a  curve  g,(.)  C  G,  which  represents  the  trajectory  of  the  i-th  node 
in  time  and  is  such  that 


gi=TeL9i-ti  =  g.Zi,i  =  1,...,£,  (4.2.1) 

where  £.(•)  G  Q,  i  =  1, . . .  We  think  of  the  g ’s  as  the  nodes  of  a  kinematic  chain,  in 

l  l 

which  case  Q  is  its  configuration  space. 

Let  the  instantaneous  shape  of  the  kinematic  chain  be  given  by  the  (£  —  l)-tuple 
(g  ,g  ,...,g  )  €  S  =  G  x  •  •  •  x  G,  where 

(£—1)  times 


9.  ... 

i,i+i 


=  9 


-l 


9  + 1  ®  1. 


(4.2.2) 


We  call  S  the  shape  space  of  the  kinematic  chain.  A  pair  of  adjacent  nodes  of  the  chain 
constitutes  a  module.  The  g  ’s  can  be  regarded  as  the  shapes  of  the  modules  of  an 

i,i+i 

[£— l)-module  kinematic  chain.  We  refer  to  the  corresponding  curves  £  c  Q  specified 

i,i+l 

by  (2.3.7)  as  the  shape  variations: 


9  —  TeLQ 

j,j+1  9i,i+ 1 


£.  .  =  g  £ 

lyl  +  1  1,^  +  1 


i  =  1,  ...,£  —  1  . 


(4.2.3) 
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We  can  generalize  the  notion  of  a  module  to  include  pairs  of  nodes  which  may  not  be 
adjacent.  Consider  the  module  that  consists  of  nodes  i  and  j  The  corresponding  shape 

g  and  £  are 

*,3  i,j 


9.  .=9.  Y  =9  ..'”9.  ,  .>  *  <3 

*>  3  i  3  M+l  J  — 1  ,3 


(4.2.4) 


and 


5.  .  =  reTs  '■£..  =  9.  ,  i<j- 

1,3  l,3  1,3  1,3 


(4.2.5) 


We  can  think  of  the  £.’s  as  characterizing  the  global  motion  of  the  G-snake  system  with 

respect  to  some  global  coordinate  system,  while  the  £  ’s  capture  the  relative  motion  (or 

hi 

shape  variation)  of  nodes  i  and  j. 

Prom  (4.2.1),  (4.2.2)  and  (4.2.3)  we  get: 


t  =  +  Ad„-i  C  ,  i  =  2,. . . , 


-l, 


2—1,1 


2-1 


(4.2.6) 


Applying  (4.2.6)  iteratively  we  can  express  any  f  as  a  function  of  f  and  of  the  shape 

l  1 

controls  £  ,...,£  as  follows: 

Sl,2’  ,s2-l,i 


c. 


Cl, 2 + 


_1,^2  — 2,2  — 1 


2,2  1,2  1 


(4.2.7) 


The  variations  in  Q  can  then  be  parametrized  either  by  (4^ £  ,...,£)  or  by 
CY  2’ ' "  ’Ci  P'  ^atter  case>  ^rst  element  (£  )  captures  the  global  mo¬ 

tion  of  the  system,  while  the  remaining  ones  2>  •  •  •  >  ^  x  e)  describe  its  shape. 

Using  (2.3.10)  we  get: 


?,  =  E-^W-4' + E4(U,^,  A 

3=1  3=1 


(4.2.8) 
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4.2.2  Nonholonomic  Constraints  and  Connections  on  Principal  Fiber  Bundles 

In  this  section  we  consider  nonholonomic  constraints  acting  on  the  G- Snake  and  we 
show  that  they  determine  a  connection  on  the  principal  fiber  bundle  associated  to  our 
problem. 

Codimension  1  Constraint  Hypothesis:  Assume  that  the  evolution  of  system  (4.2.1) 
on  each  copy  of  G  is  constrained  to  lie  on  an  (n  —  l)-dimensional  subspace  h  of  the  Lie 
algebra  G,  where  h  is  not  a  subalgebra  of  G ,  i-e. 

G/i,  i  =  1,...,£.  (4.2.9) 

Then,  for  some  A*K  £  G*  (not  necessarily  an  element  of  the  basis  (A),  i  =  1, . . .  ,n})  we 
have: 

h  =  Ker(^).  (4.2.10) 

The  constraints  (4.2.9)  can  be  expressed  as: 

A^(Q=0,  i  =  (4.2.11) 

The  constraints  (4.2.11)  are  linear  in  the  components  of  £  and  those  of  the  shape 
controls  £.  ..  This  can  be  made  explicit  by  defining  the  composite  nf  -dimensional  velocity 

'l)3 

vector  (  of  the  kinematic  chain: 

c  =  uj  •••  t1  C2  •••  e*"M)T 

1  n  1  n 

=  ■■■  ■<((,) ■ 

Theorem  4.2.1 

The  l  nonholonomic  constraints  (4.2.11)  can  be  written  in  matrix  form  as: 

\ . W(  =  0'  (42'12> 

where  A  is  a  function  of  only  the  shape  of  the  system  and  is  a  block  lower  triangular 
iv.nl  matrix  of  maximal  rank  of  the  form 


A  = 


/  *1,1 

0 

0 

0 

0 

0 

0  \ 

*1,2 

*2,2 

0 

0 

0  ••• 

0 

0 

; 

0  ••• 

0 

0 

*l,i 

*2,i 

*1-1,1 

*i,i  ■  •  ■ 

0 

0 

• 

• 

0 

V  *1,1 

*2  ,e 

*i- 1,1 

*i,i  ■  ■  ■ 

*1-1, i 

*1,1 ) 

(4.2.13) 
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with  the  1  x  n  block  *Ptq,  defined  for  p  <  q  as: 


*P,q=  ^(Adg-i  A\{Adg-xAn)  . 


p,q 


p,q 


Proof 

From  (4.2.8)  and  (4.2.11): 


n 

X«1)  =  E-4!(f,M«<A)=o. 

3  =  1 

Ai(Q  =  A) 

j=i  i=i  1  ’* 

71  71 

+  ■  ■  ■  +  E4A  +  E-^KMtH"  A). 

1=1  2,1  1=1  1>® 


(4.2.14) 


The  diagonal  blocks  *PiP  of  A  have  the  form  ^  .A*(.4i)  ■  ■  ■  A^An)  ^ ,  therefore  they 

contain  at  least  one  non-zero  constant  term.  Thus  A  has  always  maximal  rank. 


Corollary  4.2.2 

Assume  i  >  n.  After  possibly  reordering  its  elements,  we  partition  (  as  ():)),  with 
C2  an  ^-dimensional  vector  containing  the  components  of  £  (and  possibly  some  compo¬ 
nents  of  shape  variations),  while  (1  is  an  (n  —  1  ^-dimensional  vector  containing  only 
components  of  shape  variations.  Let  the  corresponding  partition  of  A  be  {A\  A2),  with 
A\  a  l  x  (n  —  1)£  matrix  and  A2  a  locally  invertible  i  x  l  matrix.  Then  from  (4.2.12): 


C2  -  ~A2  1(gi 


,2’ '  "  ‘  ‘ ' 


(4.2.15) 


The  elements  of  {1  will  be  referred  to  as  shape  controls. 

Proof 

Follows  from  the  smooth  dependence  of  A  on  the  shape  variables  and  the  maximal 
rank  property  of  Theorem  4.2.1. 
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The  physical  significance  of  this  result  is  that,  if  the  global  motion  of  the  £-node 
system  is  characterized  by  the  global  motion  of  its  first  module  (i.e.  by  £  ),  then  vari¬ 
ations  of  the  shape  (at  least  those  which  are  elements  of  £1)  induce  a  global  motion  of 
the  system. 

Definition  4.2.3  (Nonholonomic  Singularities) 

G -Snake  configurations  q  G  Q  where  the  matrix  A2  becomes  singular  for  all  possible 
partitions  (£1,  (2)  of  the  composite  velocity  vector  (,  which  are  such  that  (2  contains 
all  the  components  of  £1  (and  possibly  some  of  the  shape  variations),  will  be  called 
nonholonomic  singularities. 


There  may  be  configurations  where  the  matrix  A2  that  corresponds  to  a  particular 
partition  of  (  is  singular,  but  by  considering  a  different  partition  of  the  corresponding 
Ai  ceases  to  be  singular.  These  configurations  are  not  nonholonomic  singularities. 

Consider  now  the  manifolds  Q  and  S  defined  in  section  4.2.1  and  the  canonical 
projection  7 r  :  Q  — »  £  defined  by  equation  (4.2.2),  i.e. 

*<«! . »<)  =  K9* . =  ■  (4-2- 


Lemma  4.2.4 

The  quadruple  (Q,  S,n,G),  together  with  the  (left)  action  4>  of  G  on  Q  defined  by 
GxQ^Q:  (g,q)  =  (g,  (g^ . . .  ,g())  ^  g  ■  q  =  (gg^ . . .  ,gge)  ,  (4.2.17) 


is  a  (trivial)  principal  fiber  bundle. 

Proof 

The  canonical  projection  ■n  defined  in  (4.2.16)  is  differentiable  and  its  differential  is 


7T* 


TqQ 


Tn(q)S  :  (3^,. ..,5^)  (31>2^1]2>--->ff£_1;/£_1>£)  >  (4.2.18) 


where  the  £.  ,  are  given  by  (4.2.6): 

i— l,i 


t. 


i—l,i 


£  “  Adg-i  £.  ,  i  =  2,..., £  . 

1  i-i,i  l~1 


(4.2.19) 
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Thus,  (Q,S,n,G)  meets  the  requirements  of  Definition  2.3.21. 


Theorem  4.2.5  (Connection  for  case  i  =  n) 

Away  from  nonholonomic  singularities  and  when  l  =  n,  the  nonholonomic  con¬ 
straints  (4.2.11)  determine  a  connection  on  the  principal  fiber  bundle  (Q,  S,n,G),  with 
the  horizontal  subspace  defined  as  follows: 


Hq  =  {ve  TqQ  |  v  =  (3^,  •  -  ■  ,gt£t)  and  £.  6  h} 

(4.2.20) 

=  {u  G  TqQ  |  v  =  (s^,...,^)  and  (2  =  -A~ ^MMiMsOKi  }  , 
where  (i  =  U1'2  •••  £x'2  £2’3  •••  ^"1-f)T  and  £2  =  fc1  Q)T  . 

'1  nl  n  '  '1  n' 

Proof 

Due  to  the  left-invariance  of  our  system,  TqQ  =  {(<7^, . . .  ,<7  £  )  \  £.  G  Q).  The 
vertical  subspace  is  (from  (4.2.7),  (4.2.17)-(4.2.19)) 


Vq  =  {v  a  TqQ  |  n*q(v)  =  0} 


=  {(0^1 

•  >0^)  ^1,2^1 ,2* 

...,g  £  )  =  0} 

II 

<N 

1 — 1 

•  =  £  =0} 

(4.2.21) 

|| 

1—1 

</>v 

1-* 

‘>9,0  \t  =  Adg- 1£  i  =  2, ...,£}  . 

11  1  i,i  1 

Physically,  the  vertical  subspace  contains  all  infinitesimal  motions  of  the  kinematic  chain 
that  do  not  alter  its  shape  (those  are  not  necessarily  motions  that  satisfy  the  nonholo¬ 
nomic  constraints). 

To  show  property  (1)  of  Definition  2.3.22,  we  first  prove  that  Hq  fl  Vq  =  {0}  and  then 
that  dim(TgQ)  =  dim(/7g)  +  dim(P9). 

To  show  Hq  fl  Vq  =  {0},  assume  that  there  exists  a  non-trivial  v  E  Hqr\Vq.  By  the 
definition  of  Vq ,  the  corresponding  shape  controls  are  zero.  Thus  Ci  —  0  and,  by  the 
definition  of  Hq,  also  £2  =  0.  But  then  £  =  0  and  from  (4.2.21)  also  £,  =  0,  i  =  2, . . .  ,£. 
Thus  £  =  0.  Thus  Hq  n  Vq  =  {0}. 
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Now  observe  that,  away  from  the  nonholonomic  singularities  dim(ifg)  =  n£  —  l .  Further, 
dim(t/g)  =  n  .  So,  when  l  =  n,  dim (Hg  ©  V^)  =  ( nl  —  £)  +  n  =  ( n 2  —  n)  +  n  —  n2  = 
dim (TqQ).  It  follows  that  H q  ©  Vq  —  TqQ. 

To  show  property  (2)  of  Definition  2.3.22,  consider  Tq<&g  ■  Hq  =  g  ■  Hq  =  g  ■ 
{(9^,  ■  ■  U.  e  h}  d=  {(gg^, . .  -,ggeQ  |C.  €  h}  and  Hg.q  =  {»€  Tg.qQ  \  v  = 

(g  '  q)  ■  (Cjj-'-.Cp  and  t  e  h}  =  \  t  e  h}  .  Then,  obviously, 

Tg§g  ■  Hq  =  Hg.g. 

Property  (3)  of  Definition  2.3.22  is  immediate  from  the  smooth  dependence  of  A  on  the 
shape  and  from  the  left-invariance  of  our  system. 


4.3  Three-node  G-Snakes  on  Three-dimensional  Lie  Groups 

Here  we  specialize  the  results  of  the  previous  section  to  kinematic  chains  on  Lie 
groups  with  3-dimensional  real  non-abelian  Lie  algebras  (n  =  3).  In  section  4.3.1  we 
consider  the  Special  Euclidean  group  SE( 2),  in  section  4.3.2  the  Heisenberg  group  H( 3), 
in  section  4.3.3  the  Special  Orthogonal  group  50(3)  and  in  section  4.3.4  the  Special 
Linear  group  SL( 2). 

We  study  3-node,  2-module  kinematic  chains  on  each  of  these  groups  [l  =  n  =  3) 
by  deriving  their  Wei-Norman  representation  and  by  defining  the  partial  connection  on 
the  corresponding  principal  fiber  bundle. 

Let  G  be  one  of  the  above  four  matrix  Lie  groups  and  Q  be  the  corresponding  Lie 
algebra.  Consider  the  system  (2.3.7)  on  G  : 

9  =  TeLg  •£  =  <?£,  (4.3.1) 

with  g(.)  C  G  and  £(.)  C  Q.  The  curve  £(.)  C  Q  can  be  represented  as  in  (2.3.10): 

3  3 

=  (4-3-2) 

»=1  i= 1 

with  £  =  *4-(£)  G  2R.  From  Proposition  2.3.14,  any  g(.)  C  G  with  g(0)  =  /,  has  a  local 
Wei-Norman  representation  of  the  form  (2.3.11): 

g(t)  =  e7l(tMle72(tM2e73(tM3  _  (4.3.3) 
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The  coefficients  7*  €  IR  are  related  to  the  components  of  £  by  (2.3.12): 


7i\  /*i 

72  =  M{  71,72,73)  f2 

US 

By  differentiating  (4.3.3)  and  using  (2.3.20)  to  get: 


(4.3.4) 


dg 

dt 


=  7ie7l'4Mie72'42e73"43  +  ^2elxM  A2elzAz  +73  e^Alel2A2el3M 

=  eT'l‘Ale72^2e73>l3^ie-73-43e-72>t2i4ie72^2e73^3  +  -y2e  "73  ^  ^  g73  ^3  +^3^3] 

=  9  {^iead{-^M)ead^A^A1+^ead{-^A3)A2  +  izM  ■ 


Thus 


£  =  ^iead(-73^3)ead(-72^2)^1  +  j2ead(~KA°) A2  +  73^3  . 
From  (2.3.21)  and  (2.3.22),  we  have  for  z  =  1,2, 3  : 

AdgAi  =  ea^7l'4l^ea<i^72"42^ea^73'43^i  . 


(4.3.5) 


(4.3.6) 


and 

Adg-lAi  =  e“d(-73^3)ead(-72^l2)ea<i(-7Mi)>4.  .  (4.3.7) 

Consider  now  the  2-module  kinematic  chain  on  G.  From  the  system  kinematics  (equa¬ 
tions  (4.2.1)-(4.2.8))  we  have: 


g  —  g  g 

* 2  mm, 2 


a  —  a  a  =  a  a  a 
*3  mm, 3  mm, 2m, 3  ’ 

g  —  g  g 

m,3  m,2m,3 


From  (4.2.6)  we  get  for  the  corresponding  velocities: 


£  =  £  +  Ad, 

o  3  o  o  i 


1,3 


2,3 


2,3 


^1,2 


(4.3.8) 


(4.3.9) 
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where^ge^^.^eff. 

Assume  that  the  evolution  of  system  (4.3.1)  on  each  copy  of  G  is  constrained  to  lie  on  a 
2-dimensional  subspace  h  of  the  Lie  algebra  G,  where  h  is  not  a  subalgebra  of  Q.  Define 


Cl  =  (f1-2  e2  ci’2  c2'3  e3  e3)T  and  c2  *  {£  £  c!) 

v1  23  123/  v123' 


Theorem  4.2.1  holds  with  (  =  (£*).  Prom  Corollary  4.2.2  we  conclude  that  the  global 

velocity  of  the  2-module  kinematic  chain,  as  it  is  characterized  by  £  ,  can  be  expressed 

as  a  function  of  only  the  shape  variables  g  ,  g  and  shape  controls  £  ,  £  of  the 

1,2  2,3  1,2  2,3 

assembly: 


(2  =  ~A2  1(5l!2,92)3)^i(31|2,ff2!3)Ci  •  (4.3.10) 

Prom  Theorem  4.2.5,  equation  (4.3.10)  defines  (away  from  the  singularities  of  A2)  a 
connection  on  the  trivial  principal  bundle  ( S  x  G,  S,  n,  G)  with  S  =  G  x  G  and  with 
horizontal  subspace: 


Hq  =  {v  €  TqQ  I  V  =  ig^g^g^)  and  G  h} 


=  {u  € 


TqQ 


v  =  (g  £ 


’92C2’53C3)  andC2  =  ~a2 


K 


2,S2,3^ 


2,g2,3^  ^  ^ 


(4.3.11) 

Subsequently,  we  will  derive  explicitly  the  Wei-Norman  representation  for  each  of 
the  Lie  groups  mentioned  earlier  and  we  will  define  the  connection  (4.3.11)  for  specific 
2-dimensional  subspaces  h  of  G- 
Proposition  4.3.1  (Vershik  &  Gershkovich  [1994]) 

For  each  of  fL(3),50(3)  and  SE( 2),  all  2-dimensional  subspaces  h  of  G,  which  are 
not  subalgebras,  are  equivalent  under  Aut(G),  the  group  of  automorphisms  of  G,  and  can 
be  represented  by  h  =  sp{Ai,  A2)  in  the  basis  of  G  (specified  in  the  following  sections). 
For  SL(2),  there  are  2  classes  of  such  equivalent  subspaces  that  can  be  represented, 
respectively,  by  h  =  sp{./4i,*42}  and  by  h  =  sp{.43,./4i  +.42}- 


Due  to  this  result,  only  nonholonomic  constraints  corresponding  to  these  subspaces 
of  G  need  to  be  considered  here. 
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Our  main  purpose  in  this  section  is  to  set  the  stage  for  a  deeper  understanding  of 
this  novel  class  of  kinematic  chains,  by  cataloguing  the  low-dimensional  possibilities. 
One  case,  corresponding  to  SE( 2)  has  already  found  a  concrete  mechanical  realization 
(c.f.  chapter  5).  Others  might  follow,  for  instance,  there  are  possible  connections  be¬ 
tween  S'0(3)-Snakes  and  the  kinematics  of  long  chain  molecules  (Karplus  &:  McCammon 
[1986]). 


4.3.1  G-Snakes  on  the  Special  Euclidean  Group  SE( 2) 

Let  G  =  SE( 2)  be  the  Special  Euclidean  group  of  rigid  motions  on  the  plane  and 
Q  =  se(2)  be  the  corresponding  algebra  with  the  following  basis: 

/ o  -1  0\  / 0  0  1\  /O  0  0\ 

=  1  o  0  ,  ^2  =  0  0  0  ,  ^3=  0  0  1.  (4.3.12) 

\0  0  0/  \0  0  0/  V°  0  0/ 

Then: 

[A\  ,  ^4.2]  =  ^.3  >  [v4i  ,  ^.3]  =  —  *42 ,  [A2  ,  ^3]  =  0  .  (4.3.13) 


Proposition  4.3.2 

Let  ^(O)  =  7,  the  identity  of  G.  There  exists  a  global  representation  of  the  curve 
g(.)  C  G  of  the  form  (4.3.3)  with 


/ 71  \  /i  0  o\  /ex\ 

72  =  73  1  0  U2  •  (4.3.14) 

V73  /  \-72  0  1  /  u3y 
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Equation  (4.3.14)  is  solvable  by  quadratures: 


i 

7i (*)  —  7i(0)  +  J  r)dr  , 


t  t 

72 (t)  =  72(0)  COS  (  J  ^(ct^ct)  +73(0)  sin  (  J  ^{cr)d(^j 


it  t  t 

+  /£2(t)cos(/  ^1(cr)dCT)c/r  +  J  £3(T)sin(y 


t  t 

73 W  =  -72(0) sin  (y^^(a)da)  +73(0) cos  (  J ^(a)da\ 


Z  Z  Z  Z 

-/^)sin(/ ^(a)da)dr  +  J  ^(t)cos(^J  ^(a)dajdT  . 

Or  0  t 

(4.3.15) 


For  the  initial  condition  g(0)  =  /,  we  have  7i(0)  =0,  i  =  1,2,  3. 


Proof 

Since  Q  =  se(2)  is  solvable,  the  existence  of  a  global  representation  is  immediate  by 
(Wei  &  Norman  [1964]).  To  see  that  it  has  the  form  (4.3.3)  with  coefficients  given  by 
(4.3.14)  and  (4.3.15),  we  compute  the  RHS  of  (4.3.5),  using  (4.3.13)  and  (2.3.20): 


ead(-j3A3)ead(-^A2)Ai  =  ead(-l3A3)  ^  +  ^3)  =  ^  ^  +  ^  > 

ead(-l3A3)A2  = 


(4.3.16) 


From  (4.3.1),  (4.3.2),  (4.3.5)  and  (4.3.16),  we  have: 


£  =  £i4.i  +  £2-A2  +  £3.4.3 

=  71  Ml  -  73^2  +  72^3)  +  72*42  +  73^3  =  7i4.i  +  (72  -  7371)^2  +  (73  +  7271)^3  • 


Since  {Ai,  A2, 4.3}  is  a  basis,  we  have: 


£i=7i,  £2  =  72  -  737i  ,  £3  =  73  +  727i  • 
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Solving  for  the  7»’s  we  get  (4.3.14),  which  can  be  rewritten  as: 


7i  =  fi  » 

(£)-(4,  o)(")+(D- 

This  system  can  be  solved  by  quadratures,  giving  (4.3.15). 

■ 

Lemma  4.3.3 

Consider  the  Wei-Norman  representation  (4.3.3)  of  g  6  SE( 2)  determined  by 
(4.3.14)  and  (4.3.15).  Then: 

AdgAi  =  +  (72  sin 71  +  73  cos7i).42  +  (-72  cos 71  +  73  sin 7i)43  , 

AdgAi  =  cos  71^2  +  sin 71^3  ,  (4.3.17) 

AdgAz  =  -  sin  7!  .4,2  +  cos  7^3  . 

Moreover, 

Adg-iAi  =  Ai  —  73^2  +  72^3  ) 

Adg-iA2  —  cos  71X2  -  sin 71^3  ,  (4.3.18) 

Adg-iAz  =  sin 71^.2  +  cos7i43  . 

Proof 

We  need  to  compute  (4.3.6).  For  Ai  we  have: 

ead{l3A,)Al  =  +  73^3 

ead(y2A2)ead(j3A3)Ai  =  ^  ^  +  ^ 

ead('nAi)ead('y2A2)ead(j3A3)A1  —  Al  _|_  (72sin7i  +73COS7i)4-2 

+  (-72  COS71  +  73  sin 7i)*43  . 

For  4.2  : 

ead{l3A3)A2  = 
ead{l2A2)ead{l3A3)A2  = 

ead(7M1)ead(72^2)e«d(73^3)>/42  =  COS7l>42  +  Sin7lA3  ■ 
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For  A3  : 


ead^A^A3  =  A3 

ead(j2A2) ead{'y3A3) ^ 

ead(7Mi)ead(72>l2)ead(73^3)^3  =  -  gin 7i^2  +  COS  71^3  . 

From  the  above,  (4.3.17)  is  immediate. 

We  also  need  to  compute  (4.3.7).  For  Ai  we  have: 

ead(~^A^Ai=Ai 

ea<i(— 72.42)ea<i(-7i>li)_41  _  ead(-12A2)  ^  ^  + 

ead(-73^3)ead(-72^2)ea<i(-7i>li)>41  =  ^(-73^3) +  ^3)  =  A  ~  73^2  +  72^3  ■ 

For  .42  : 

ead(-'nA1 )a2  =  cos7i^42  -  sin  71.43 

ead(-y2A2) ead(—nA2) _  ead(-'y2A2 ) (c0S 'y1*^2  -  sin  71.43)  =  COS7i^42  -  sin7i^43 
ead(-73^3)ea<i(-72-42)ea<i(-7i-4i)v42  =  ead(~73  A})  (cos  -  Sin  71.43) 

=  cos7i^12  -  sin  7i^3  . 

For  .4.3  : 

eai(-7l4i)^3  _  sin^lv42  +  cOS7i^43 

gfld( — 72  A2 )  gad( — 71  .A3 ) _  gflrf(  72  A2 )  ^gjn  71 .4  2  +  COS  71.43)  =  sin7i.42  +  cos  71A.3 
ead(—y3A3)ead(-'f2A2)ead(-jiA1)^3  _  ead(--r3A3)  (sjn/ylv/42  +  COS  71  A3) 

=  sin  71  A2  +  cos  71 A3  . 

From  the  above,  (4.3.18)  is  immediate. 


Using  the  definition  of  the  basis  {Ai}  from  (4.3.12),  we  have: 

/  cos  71  —  sin  71  0  \  / 1  0  72  \  / 1  0  0  \ 

e71'4!  =  |  sin7i  cos  71  0  1  ,  e72,42  =  I  0  1  0  ]  ,  e73-43  =  (  0  1  73  ) 

\  0  0  1 J  \0  0  1  /  \0  0  1  / 

(4.3.19) 
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Then,  from  (4.3.3): 


g  =  e7i>lie72^2e73>l3  _ 


(cos  7i 
sin7i 
0 


—  sin  71 

COS71 

0 


72COS71  -  73  sin  7!  \ 
72  sin7i  +  73  cos  71 

1  J 


(4.3.20) 


Define: 


,  def 
4>  =  71 


def 


x  =  72  cos  71  -  73  sm7i 
t 


Xo  +  J £2(t)  cos (/>(r)  dr  -  J £3(t)  sin <f>(r)  dr  , 


(4.3.21) 


uei 

y  =  72  sin  7!  +73COS71 


dr  + 


t 

I  cos  4>(r)  dr  , 
0 


where  x0  =f  72  (0)  cos  71  (0)  -  73(0)  sin  71(0)  and  y0  d=  72(0)  sin  71(0)  +  73(0)  cos  71(0). 
Then  we  take  from  (4.3.20)  the  usual  form  of  elements  of  SE( 2) 


(cos  (j) 
sin  (f> 

0 


—  sin  (p 
cos  (j) 


0 


(4.3.22) 


Differentiating  and  using  (4.3.14),  we  have: 


x  =  (2  cos  71  -  6  sin7i  , 

V  =  6  sin  7a  +6  cos  71  ,  (4.3.23) 

Observe  that 

71  =<t>, 

72  =  xcoscj)  +  ysmcj) ,  (4.3.24) 

73  =  —a;  sin  (j)  +  y  cos  (f> 
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and 


6  =  <f> , 

£2  =  x  cos  4>  +  y  sin  4> , 
£3  =  —x  sin  (f)  +  y  cos  (p  . 


(4.3.25) 


Consider  now  the  3-node  kinematic  chain  on  SE( 2).  A  concrete  mechanical  real¬ 
ization  of  such  a  system  is  the  Variable  Geometry  Truss  assembly  detailed  in  the  next 
chapter.  The  system  kinematics  of  equations  (4.3. 8), (4. 3. 9)  apply. 

From  (4.3.13)  we  can  see  that  there  are  at  least  two  possible  2-dimensional  subspaces  h 
of  Q  that  can  generate  the  whole  algebra  under  Lie  bracketing: 

h3  =  sp{.di ,  wd2}  =  Ker^g)  and  h2  =  sp{.4i,.A3}  =  Ker^g)  .  (4.3.26) 

From  Proposition  4.3.1  we  know  that  those  subspaces  are  equivalent  under  automor¬ 
phisms  of  Q  (e.g.  for  £  €  se(2)  with  coordinates  (£l(£2,£  )  with  respect  to  the  basis 
defined  in  (4.3.12),  consider  the  automorphism  ip  :  se(2)  — I  se( 2)  :  (C^ C2> £g) 
(£i)— £3>£2)>  not'ng  that  ip(h3)  =  /i2  ).  Thus,  subsequently,  we  will  consider  only  the 

subspace  h2,  The  nonholonomic  constraints  £.6  h2  can,  then,  be  expressed  as: 

i 

4(0=0,  <  =  1,2,3.  (4.3.27) 

l 

The  exact  form  of  Ai,A2,(i  and  (2,  as  well  as  a  description  of  the  system’s  non¬ 
holonomic  singularities,  is  presented  in  section  5.3,  as  well  as  possible  motion  control 
schemes  based  on  periodic  shape  variations. 

4.3.2  G-Snakes  on  the  Heisenberg  Group  H{ 3) 

Let  G  =  H( 3)  be  the  Heisenberg  group  of  real  3x3  upper  triangular  matrices  of 
(l  a  (3\ 

the  form  0  1  7  and  let  Q  =  h( 3)  be  the  algebra  of  3  x  3  nil-triangular  matrices 

\o  0  1 J 

with  the  following  basis: 

(0  1  0\  / 00  0\  / 0  0  1\ 

Ai  =  0  0  0  ,  A2  =  0  0  1,^3  =  0  0  0  .  (4.3.28) 

\0  00/  \0  0  0 /  \0  0  0/ 
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Then: 

Hi  ,  M  =  -43  ,  Hi  ,  A3]  =  o  ,  [A2  ,  A3]  =  0  .  (4.3.29) 

Proposition  4.3.4 

The  algebra  Q  =  h{Z)  is  nilpotent  (thus  solvable)  and,  from  Proposition  2.3.14,  any 
g  £  G  =  H{ 3)  has  a  global  Wei-Norman  representation  of  the  form  (4.3.3)  with 


71  \  /  1  0  0\ 

72  =  0  10 

73  /  V  "72  0  l ) 

Equation  (4.3.30)  is  solvable  by  quadratures: 


(4.3.30) 


7iM  =  7i(0)  +  J  ^(rjdr  , 

o 

t 

72 (<)  =72(0)  +  J  Z2{t)(It  , 

(4.3.31) 

t  t 

73(f)  =  73(0)  -  J 72 (r)C1(r)dr  +  J ^(r)rfr 

0  0 

t  t  T  t 

=  73(0) -72(0)  J  J  £1(t)^J  Z2{a)daj  dr  +  J  ^(r) dr  . 

000  0 


Proof 

Since  £  =  h(3)  is  nilpotent,  the  existence  of  a  global  representation  is  immediate  by 
(Wei  &  Norman  [1964]).  To  see  that  it  has  the  form  (4.3.3)  with  coefficients  given  by 
(4.3.30)  and  (4.3.31),  we  compute  the  RHS  of  (4.3.5),  using  (4.3.29)  and  (2.3.20): 

eadl~i2A7)Al  =  Al  +  l2A3, 

ead(-y3A3)ead(-^A2)Ai  =  ^  +  ^  ,  (4.3.32) 

ead{"*A*)A2  =  A2  • 

From  (4.3.1),  (4.3.2),  (4.3.5)  and  (4.3.32),  we  have: 

£  —  Cl^l  +  ^2^-2  +  ^3^.3 
=  7lHl  +  72«43)  +  72^2  +  73^3  • 
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Since  {Ai,A2,As}  is  a  basis,  we  have: 


Ci  =  7i  ,  6  =  72  ,  6  =  727i  +  73  ■ 


Solving  for  the  7j’s  we  get  (4.3.30).  This  system  can  be  solved  by  quadratures,  giving 
(4.3.31). 


Lemma  4.3.5 

Consider  the  Wei-Norman  representation  (4.3.3)  of  g  e  H( 3)  determined  by  (4.3.30) 
and  (4.3.31).  Then: 

Adg-iAi  =  Ai  +  72^3  ) 

Adg-iA2  =  A2  —  71^3  >  (4.3.33) 

Adg-iAa  =  *43  . 

Proof 

We  need  to  compute  (4.3.7).  For  Ai  we  have: 

ead(- yiA,)Al  =  Al 
ead(-j2A2)ead(-^Ai)Al  =  Al  +72v43 

ead(-73^3)ead(-72^2)eod(-7i^i)^1  =  ^  +  72*43  . 

For  ^42  : 

e««*(-7i^i)^2  =  (42_7l<43 
ead(-72^2)ead(-7i*42)>42  =  ^ 

ead(-73^3)earf(-72^2)ead(-7i^i)t42  _  ^  _  7l43  _ 

For  .43  : 

^(-7^)^  = 
ead(-72^2)ead(-7i^3)>43  =  ^ 

ead(-73*t3)ead(-72X2)eod(-7Mi)^3  _  A 

From  the  above,  (4.3.33)  is  immediate. 
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Using  the  definition  of  the  basis  {-4;}  from  (4.3.28),  we  have: 


P7Ml  _ 


1  7i  0 
0  1  0 
0  0  1 


£>72-4.2 


d73-43 


Then,  from  (4.3.3): 


g  =  e'nAl  e72^2e73^3 


1  7i  7i72  +  73 


=  (0  1 
0  0 


72 

1 


(4.3.34) 


Consider  now  the  3-node  kinematic  chain  on  G  =  H( 3).  The  system  kinematics  of 
equations  (4. 3. 8), (4. 3. 9)  apply. 

From  (4.3.29)  we  can  see  that  there  is  at  least  one  possible  2-dimensional  subspace  h  of 
Q  that  can  generate  the  whole  algebra  under  Lie  bracketing: 

h  =  sp{-4i,-42}  =  Ker^)  .  (4.3.35) 

The  nonholonomic  constraints  can,  then,  be  expressed  as: 

-4(£.)  =  0,  i  =  1,2,3.  (4.3.36) 

Equation  (4.3.10)  holds  with  (i  and  (2  defined  as  above  and  with: 


Ai 


/  0  0  0 

I  0  0  1 

l  A\(Adg-i  -4i)  A\(Adg-i  >42)  ^(^Id^-i  .43) 

v  2,3  2,3  2,3 

/  0  0  0  0  0  0\ 

0  0  1  0  0  0 

\722’3  -7i’3  10  0  1/ 


0  0  0\ 
0  0  0] 
001/ 


and 


42 


(  0 
A^Adg-1  -40 
1,2 

Ai(Adg-i  -4i) 

\  1,3 


1 

1 

1 


0 

A\{Adg-^A2) 

•4-3  {Adg- 1  -42) 

1,3 


1  \ 

•A  (-4dg-l  -4s) 

1  2 

A\(Adg-i  -43) 

1,3  / 
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The  nonholonomic  singularities  of  the  system  are  the  configurations  where: 


tet{A2)  =  A\(Adg-^Ai)A\(Adg-.A2)  -  A\(Adg:lAi)A^(Adg:lA2) 


1,3 


1,2 


1,2  1,3  ,  1,2  1,3  1,2  2,3  .  1,2  2,3  n 

=  -72  7i  +  7i  72  =  -72  7i  +7i  72  =  0 


4.3.3  G— Snakes  on  the  Special  Orthogonal  Group  SO (3) 

Let  G  =  50(3)  be  the  Special  Orthogonal  group  of  real  orthogonal  3x3  matrices 
with  determinant  equal  to  one  and  let  Q  —  so(3)  be  the  algebra  of  3  x  3  real  skew- 
symmetric  matrices.  Consider  the  following  basis  for  Q  : 

f  0  0  0  \  /  0  0  1\  / 0  -1  0\ 

Ai  =  0  0  -1  ,  A2  =  0  0  0  ,  Az  =  1  0  0  .  (4.3.37) 

V 0  1  0  /  \-l  0  0/  \0  0  0/ 

Then: 

[*4i  ,  A?]  =  A3  ,  [^4i  ,  A3]  =  —A2  ,  [.A2  ,  .4.3]  =  A\  .  (4.3.38) 

Proposition  4.3.6 

Let  g( 0)  =  I,  the  identity  of  G  =  50(3).  The  algebra  Q  =  so(3)  is  simple,  thus, 
the  Wei-Norman  representation  (4.3.3)  is  only  local  (defined  when  cos  72  ^  0)  with 
coefficients: 


7i\  | 

(  sec  72  cos  73 

-  sec  72  sin  73 

72  = 

sin  73 

cos  73 

73/ 

V  -  tan  72  cos  73 

tan  72  sin  73 

(4.3.39) 


Proof 

To  see  that  the  Wei-Norman  representation  has  the  form  (4.3.3)  with  coefficients 
given  by  (4.3.39),  we  compute  the  RHS  of  (4.3.5),  using  (4.3.38)  and  (2.3.20): 

ead(-72^2)^i  _  cos 72“4i  +  sin 72^3  , 

ead(-73^3)e<id(-72^2)^1  _  cos72cos73*4i  -  cos 72  sin 73^2  +  sin 72^3  ,  (4.3.40) 

ead(-'f3A3)j_2  _  sjn 73v41  +  cos  73^2  • 


97 


Prom  (4.3.1),  (4.3.2),  (4.3.5)  and  (4.3.40),  we  have: 

£  =  £i-4i  +  £2-4.2  +  £3.4.3 

=  71  (cos  72  cos  73.4 1  -  cos  72  sin73^42  +  sin  72-43) 

+  72 (sin  73^.  1  +  cos73)^42)  +  73^3  . 

Since  {Ai,A2,Aa}  is  a  basis,  we  have: 

£1=71  cos  72  cos  73  +  72  sin  73  ,  £2  =  -71  cos  72  sin  73  +  72  cos  73  ,  £3  =  71  sin  72  +  73  • 
Solving  for  the  7j’s  we  get  (4.3.39). 

■ 

Lemma  4.3.7 

Consider  the  Wei-Norman  representation  (4.3.3)  of  g  E  SO( 3)  determined  by 
(4.3.39).  Then: 

Adg-iAi  =  cos  72  cos  73^!  -  cos  72  sin 73.4.2  +  sin 72^3  , 

Adg-iA2  —  (sin 7i  sin 72  cos  73  +  cos  7x  sin 73)^1 

+  (-sin7i  sin 72  sin 73  +  cos  71  cos  73)^2  -  sin  7:  cos  72 A3  , 
Adg-xAz  =  (-COS71  sin  72  cos  73  +  sin  71  sin  73)^1 

+  (cos 71  sin 72  sin 73  +  sin 71  cos 73M2  +  cos 71  cos 72^3  . 

(4.3.41) 

Proof 

We  need  to  compute  (4.3.7).  For  Ai  we  have: 
ead(-^A1)Al  =a1 

ead(-n/2A2)ead(-y1A1)Ai  =  COS  7^  +  sin  72^3 

gad(— 73^3 ) gad(— 72  A2 ) Qad{ —71  *4i )  —  COSy2  cos  73 .4 1  —  cos  72  sin73-42  +  sin 72^3  . 

For  A2  : 

ead(-7i Ai)A2  _  cos 7^2  —  sin 7i-43 

ead(— 72  a2  )  71  -^2 =  sin7i  sin72-4i  +  cos  71 -42  —  sin 71  cos72-43 

ead(—y3A3)ead(-'y2A2)ead(-j1Ai)A2  _  sin 72  COS  73  +  COS  71  sin 73)^1 

+  (-  sin7i  sin  72  sin  73  +  cos  73  cos  73)^2  -  sin  7!  cos  72^.3  , 
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For  A3  : 

ead{-'nAi)^s  _  sjn,y1(42  -).  cos 71^4.3 

ead(--y2A2) ^di-'nAs) _  _CQS^1  gjn-^^  -j-  sin7i^42  +  COS71  COS  72-43 
ead(—y3A3)ead(-'y2A2)ead(-'y1Ai)^3  _  cog  ^  sin-y2  COS  73  +  sill7i  sin 73)^1 
+  (COS71  sin  72  sin  73  +  sin  71  cos  73)^2  +  cos  71  cos  72  A3  . 

From  the  above,  (4.3.41)  is  immediate. 

■ 


Using  the  definition  of  the  basis  {Ai}  from  (4.3.38),  we  have: 


d7i-Ai  _ 


1 

0 

0  ^ 

|  ,  e72-42  =  | 

(  cos  72 

0 

sin  72  \ 

b 

cos  71 

-sin7i 

0 

1 

° 

\o 

sin  71 

cos  71  j 

V  -  sin  72 

0 

cos  72  / 

Then,  from  (4.3.3): 


g—  g7l-4l  g"72  A2  g73  Ai 


cos  73  -sin  73  0 

073-43  —  I  sin  73  cos  73  0 

0  0  1 


cos  72  cos  73  -  cos  72  sm  73  sin  72 

=1  cos  71  sin  73  +  sin  71  sin  72  cos  73  cos  71  cos  73  —  sin  71  sin  y2  sin  73  —  sin  71  cos  72 
\sin  71  sin  73  —  cos  71  sin  72  cos  73  sin  71  cos  73  +  cos  71  sin  72  sin  73  cos  71  cos  y2 

(4.3.42) 

Consider  now  the  3-node  kinematic  chain  on  G  =  50(3).  The  system  kinematics  of 
equations  (4. 3. 8), (4. 3. 9)  apply. 

From  (4.3.38)  we  can  see  that  there  are  at  least  three  possible  2-dimensional  subspaces 
h  of  Q  that  can  generate  the  whole  algebra  under  Lie  bracketing: 

h3  =  sp{Ai,-A2}  =  Ker^)  ,  h2  =  sp{^i,^3}  =  Ker^), 

(4.3.43) 

and  hi  =  sp{^t2, -A3}  =  Ker(Ai). 


From  Proposition  4.3.1  we  know  that  those  are  equivalent,  so  we  consider  only  h3  C  Q. 

The  nonholonomic  constraints  £.  €  h3  can,  then,  be  expressed  as: 

i 


-4(^.)  =  0,  *  =  1,2,3. 


(4.3.44) 
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Equation  (4.3.10)  holds  with  £1  and  (2  defined  as  above  and  with: 


0  0  0  0  0  O' 

0  0  1  0  0  0 

A\{Adg-iAi)  A\{Adg-iA2)  A\(Adg-^A3)  0  0  1 


2,3 


2,3 


0  0 

0  0 

2,3  •  2,3  2,3 

sin72  —  sm71  cos72 


2,3 

0 

1 

2,3  2,3 

cos  7j  cos72 


and 


Ao  — 


(  0  0  1  \ 

-43  ( Adg -1  ^4i)  A\  (Adg-1  A2)  A\  (Adg- 1  -43) 

1,2  1,2  1,2 

A3(Adg-l  Al)  -43  [Adg-1  A2)  A3(Adg-i  A3) 

1,3  1,3  1,3 


0 


0 


1 


12  *12  12  12  12 
=  I  sin72’  —  sin7j’  cos72’  cos 77  cos72’ 

sin72’3  —  sin71’3  cos72’3  - ’3 - ’3 


cos71’  cos72 

The  nonholonomic  singularities  of  the  system  are  the  configurations  where: 


det(^2)  —  A^ (Adg-i  AijA\i(Adg-i  -42 )  —  A\(Adg-i  Ai)Ai3[Adg-\  A2) 

1,2  1,3  1,3  1,2 


4.3.4  G-Snakes  on  the  Special  Linear  Group  SL( 2) 

Let  G  =  SL( 2)  be  the  Special  Linear  group  of  real  2x2  matrices  with  determinant 
one  and  let  Q  =  sl( 2)  be  the  algebra  of  real  2x2  matrices  of  trace  zero.  Consider  the 
following  basis  for  Q  : 


Then: 


~(l 

2  VO 


(4.3.45) 


[Ai  ,  A2]  —  2-43  j  [«4i  ,  A3\  —  —  A\  ,  [-42  ,  -43]  =  -42 .  (4.3.46) 
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Proposition  4.3.8 

Let  g( 0)  =  7,  the  identity  of  G  =  SL( 2).  The  Wei-Norman  representation  (4.3.3)  is 
only  local  (defined  when  e73  A  0)  with  coefficients: 

(  0  /«: i\ 

7|e73  e-73  0  U  .  (4.3.47) 

V-272^3  0  l)  \ij 

(See  however  comments  in  (Wei  &  Norman  [1964])  and  their  Theorem  3.  A  global 
representation  of  SL( 2)  can  be  obtained  using  -  -4.2,  -^.3}  as  a  basis). 

Proof 

To  see  that  the  Wei-Norman  representation  has  the  form  (4.3.3)  with  coefficients 
given  by  (4.3.47),  we  compute  the  RHS  of  (4.3.5),  using  (4.3.46)  and  (2.3.20): 

ead(-^A2)Ai  =Ai_  +  2 72i43  5 

ead(-KA3)ead(-l2A2)Ai  =  e~^Al  _  ^  A2  +  27243  ,  (4.3.48) 

ead(-y3A3)A2  =  e73>42  . 

Prom  (4.3.1),  (4.3.2),  (4.3.5)  and  (4.3.48),  we  have: 

£  =  Ci4i  +  £242  +  £3-A3 

=  7i(e_734i  -  7|e7342  +  27243)  +  72e7342  +  7343  . 

Since  {v4i,4.2,.A3}  is  a  basis,  we  have: 

6  =  7ie_73  ,  6  =  -7i 72e73  +  72e73  ,  £3  =  7i272  +  73  . 

Solving  for  the  7i’s  we  get  (4.3.47). 


Lemma  4.3.9 

Consider  the  Wei-Norman  representation  (4.3.3)  of  g  =  SL( 2)  determined  by 
(4.3.47).  Then: 

Adg-iAi  =  e-734i  -  7fe7342  +  27243  , 

Adg-iA2  =  -7?e_734i  +  (7i72  +  l)2e7342  -  27i(7l72  +  1)43  ,  (4.3.49) 

Adg-iA3  -  7ie“734i  -  72(7i72  +  l)e73-42  +  (27l72  +  1)43  . 
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Proof 


We  need  to  compute  (4.3.7).  For  Ai  we  have: 

ead(-7i  M)Al=Al 

ead(-72^2)ead(-7M1)>4l  =  ^  ^  +  3^ 

ead(-73^3)ead(-7^2)ead(-7i^l)^1  =  g-73^  _  +  272.A3  . 


For  A2  : 


ead(-^A1)A2  =  _72^1  +  -  27l^3 

ead(-72^)ead(-7M2)^2  =  +  (1  +  7l72)2^2  _  27l(l  +  7l72)^3 

ead(-73^3)ead(-72>l2)ead(-71^x)>42  =  -^g-73^  +  (t  +  7l72)VM2 

-  27i(1  -t-  7i72)-4.3  • 


For  A3  : 

e«  d(-'nA1)A3  =  7lA1  +  A3 

ead(-72^2)ead(-7M3)^3  =  7liAl  _  72(!  +  7l72)^2  +  (1  +  27l72)^l3 

ead(-73^)ead(-72^2)ead(-7l^1)>43  =  y^-Ts  Al  -  y2(l  +  7l72)e^M2  +  (1  +  27ll2)A3  • 

From  the  above,  (4.3.49)  is  immediate. 


Using  the  definition  of  the  basis  {Ai}  from  (4.3.45),  we  have: 


=71 


At  _  f  1  7i 
0  1 


o72  A2  _  ^  } 

^73  A3  _  (  cosh  73  +  |sinh73 

"V  0 


0 

cosh73  —  \  sinh73 


Then,  from  (4.3.3): 


a  =  g7i Ai  g72  A2  e73 A3  _  ( (!  +  7i72)(cosh73  +  \  sinh73)  7i(cosh73  -  \  sinh7s)  \ 

’  \  72(cosh73  +  I  sinh73)  cosh 73  -  |  sinh 73  ) 

(4.3.50) 
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Consider  now  the  3-node  kinematic  chain  onG  =  SL( 2).  The  system  kinematics  of 
equations  (4. 3. 8), (4. 3. 9)  apply. 

From  (4.3.49)  we  can  see  that  there  are  at  least  two  possible  2-dimensional  subspaces  h 
of  G  that  can  generate  the  whole  algebra  under  Lie  bracketing: 

h3  =  sp{^4i ,  ^42}  =  Ker(4)  and  hii2  =  sp{^t3,^li  +  A2}  =  Ker(4  -  4)  .  (4.3.51) 


From  Proposition  4.3.1  we  know  that  those  are  not  equivalent.  We  first  consider  h3  C  Q. 

The  nonholonomic  constraints  £  £  h3  can,  then,  be  expressed  as: 

1 


4(f)  =  0,  i  =  1,2,3. 


(4.3.52) 


Equation  (4.3.10)  holds  with  (i  and  (2  defined  as  above  and  with: 


and 


Ai  — 


0  0  0  0  0  O' 

0  0  1  0  0  0 

A\(Adg-i  A\{Adg-xA2)  A\(Adg-xA3)  0  0  1 


2,3 


2,3 


2,3 


0  0  0  0  0  0 

0  0  1  0  0  0 

,2722'3  -27l2’3(7i’3722’3  +  1)  27l2’372’3  +  1  0  0  1, 


Ao  — 


(  0  0  1  \ 

A\{Adg-iAi)  A\(Adg-xA2)  AUAdg-iAa) 
1,2  1,2  1,2 

A^3(Adg-l  A\)  A3{Adg-l  A2)  A3  (Adg-l  A3) 


1,3 


1,3 


1,3 


C  -27l,3(7i’3i,3  +  1}  27l375’3  +  1 

y2  -2ll'3(A3A3  +  1)  27}’372’3  +  1 

The  nonholonomic  singularities  of  the  system  are  the  configurations  where: 

det(yl2)  =  Ab3(Adg- 1  AAA^iAdg- 1  A2)  -  A\(Adq-x  Ai)A\(Adq-i  A2) 

1,2  1,3  1,3  1,2 

=  -4721’27i1’3(7i1’372'3  +  1)  +  4721’37i1’2(7i1>2721’2  +  1)  =  0  . 


1 

1,2  1,2 


Now  we  consider  the  subspace  h\i2  C  G  in  (4.3.51).  The  nonholonomic  constraints  can, 
then,  be  expressed  as: 

(4-4)(f)=0,  *  =  1,2,3.  (4.3.53) 
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Equation  (4.3.10)  holds  with  (i  and  (2  defined  as  above  and  with: 


A\  — 


0  0 

1  -1 

(. A  -  A)  (Mr-  a,)  (A  -  A)  (M,-1  a2) 

2,3  2,3 

0  0  0  0 
0  0  0  0 
(A-A)(Adg-iA&)  1  -1  0 

2,3 

0  0 

e-732,3  +  (72.3)2e732-3  _(72.3)2e-732'3  _  (-y^3-^-3  +  l)2e ^ 

0 
0 

7i  e  ^  +7z>  (7l-  7a>  +  l)eT3  i 


and 


/ 


^2  — 


(4- 

(4- 


i 

A)(Adg-iAi) 

1,2 

A){Adg->A1) 

1,3 


-1 

(4  -  A)  {Adg-,A2) 
{A  -  A)  {Adg-i  A2) 


1,3 


°  \ 

—  A2)  {Adg-l  ^3) 

{A-AKAdg-lAs)  J 

1,3  / 


=  e  73 


1,2 


--1’3 

e  73 


+  (721,2)2^ 

+  (72,3)2e7* 


■(7i’2)2e  73 ' 

-(7i1’3)2e-^1’3 


-1 

-  (ll’VA  +  l)2e73 

-(7i1’3721’3  +  l)2e731 


1,3 


7i1,2e" 

7i1,3e" 


1,2 


0 


■7r  +721,2(7i1,272’2  +  l)e73’ 
73'3+721’3(7i1’3721’3  +  l)e73’ 


The  nonholonomic  singularities  of  the  system  are  the  configurations  where: 


det(^2)  =  (A[  -  A)  (Mr1  (^1  +  ^2))  (4  -  A)  {Adg-i  A3) 


1,2 


1,3 


-  {A  -  A)  {Adg-x  ( A 1  +  A2))  (A  -  A)  {Adg-1  As) 

1,3  1,2 
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=  (e  731’2  +  (l22)2^’2  -  (7i,2)2e  73,S  -  (7i1,2721,2  +  l)^7*’2) 


7j’3e-7*’3  +  72’3(7i’372’3  +  l)e7® 


e-7*'3  +  (72’3)2e73’3  -  (7x1>3)2e-73’3  -  +  l)2^ 

■  ^7i,2e-7*'2  +721,2(7i1,272’2  +  Ije7*’*^  =  0  . 
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CHAPTER  FIVE 


NONHOLONOMIC  VARIABLE  GEOMETRY  TRUSS 
ASSEMBLIES 


5.1  Introduction 

In  this  chapter  we  consider  an  instance  of  £E(2)-Snakes,  based  on  a  particular 
type  of  Variable  Geometry  Truss  (VGT)  assemblies  (Miura,  Furuya  &  Suzuki  [1985]; 
Wada  [1990]).  These  are  structures  consisting  of  longitudinal  repetition  of  similar  truss 
modules.  In  the  present  instance,  each  module  is  implemented  as  a  planar  parallel 
manipulator  consisting  of  two  platforms  connected  by  legs  whose  lengths  can  vary  under 
the  control  of  linear  actuators.  Each  platform  is  equipped  with  a  pair  of  wheels,  so  that 
it  can  move  on  the  plane  that  supports  the  structure  (fig.  5.1.1).  The  wheels  of  each 
platform  are  free  and  not  actuated  and  their  motion  is  independent  of  each  other,  while  we 
assume  that  the  wheels  roll  without  slipping  on  the  plane.  This  imposes  a  nonholonomic 
constraint  on  the  motion  of  each  platform,  namely  the  requirement  that  its  velocity  is 
perpendicular  to  the  axis  connecting  the  wheels.  When  the  legs  of  the  individual  modules 
are  expanded  or  contracted,  the  shape  of  the  whole  structure  changes.  As  a  consequence 
of  the  nonholonomic  constraints  imposed  by  the  rolling-without-slipping  assumption 
on  the  wheels,  this  shape  change  induces  a  global  motion  of  the  structure,  as  we  shall 
show.  These  structures  will  be  called  Nonholonomic  Variable  Geometry  Truss  (NVGT) 
assemblies. 

The  motion  planning  problem  for  such  an  assembly  is  of  the  nonholonomic  variety. 
There  is  a  significant  body  of  research  related  to  such  problems  (see  e.g.  (Latombe  [1991]; 
Li  &  Canny  [1993];  Murray,  Li  &  Sastry  [1994])),  which  in  general  assumes  cart-type 
mobile  robots  moving  under  direct  actuation  of  a  set  of  wheels.  The  main  difference  in  our 
case  is  the  prominence  of  shape  changes  as  the  means  which,  together  with  the  action  of 
the  nonholonomic  constraints,  induces  global  motion  of  the  system.  This  is  analogous  to 
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Fig.  5.1.1:  One  module  of  the  NVGT  assembly 


the  idea  of  reorientation  in  free-floating  multibody  systems,  induced  by  closed  joint  space 
trajectories  under  the  nonholonomic  constraint  of  conservation  of  angular  momentum 
(Krishnaprasad  [1990];  Krishnaprasad  &  Yang  [1991];  Marsden,  Montgomery  &  Ratiu 
[1990]). 

VGT  assemblies  have  been  examined  in  the  past  (see  (Chirikjian  &  Burdick  [1991]; 
Wada  [1990])  and  references  there),  but  the  emphasis  was  on  its  capabilities  as  a  re¬ 
dundant  manipulator  and  on  locomotion  using  snake-like  motions,  not  on  the  special 
problems  introduced  by  nonholonomic  constraints.  A  system  similar  to  the  one  de¬ 
scribed  here  was  built  by  (Chirikjian  &  Burdick  [1993])  using  castors  instead  of  wheels 
in  the  platforms  of  the  modules  and  therefore  the  nonholonomic  constraints  that  we 
consider  here  were  not  present. 

In  section  5.2,  we  examine  the  kinematics  of  an  NVGT  assembly  with  i  platforms 
and  i  —  1  modules.  Each  platform  can  be  considered  as  a  node  of  a  kinematic  chain.  The 
configuration  of  the  £-node  NVGT  assembly  can  be  described  by  its  shape  and  by  the 
position  and  orientation  of  the  assembly  with  respect  to  some  fixed  (world)  coordinate 
system,  thus  by  a  total  of  l  elements  of  SE( 2).  Consider  the  i-th  module  that  includes 
the  i-th  and  ( i  +  l)-th  platforms  (fig.  5.1.1).  Its  shape  can  be  described  by  the  relative 
position  and  orientation  of  the  coordinate  frame  centered  at  the  point  Ol+i  with  respect 
to  the  coordinate  frame  centered  at  the  point  O,.  Then,  the  shape  of  each  module  corre¬ 
sponds  to  an  element  of  the  Special  Euclidean  group  SE( 2)  that  describes  rigid  motions 
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on  the  plane  and,  as  a  result,  the  shape  of  the  (£  -  l)-module  NVGT  can  be  described 
by  (£  —  1)  elements  of  SE( 2).  In  (Brockett,  Stokes  &  Park  [1993])  a  systematic  way  of 
deriving  the  kinematics  of  serial  linkages  is  presented  based  on  the  “product  of  exponen¬ 
tials”  formula,  where  the  configuration  of  the  system  is  described  by  an  element  of  the 
appropriate  SE(n)  group  and  is  expressed  as  a  product  of  its  one-parameter  subgroups, 
with  one  element  of  the  product  corresponding  to  each  of  the  one-degree-of-freedom 
joints  of  the  linkage.  The  NVGT  assembly  that  we  consider  here  is  a  structure  similar 
to  the  ones  described  there,  but  the  joints  are  more  complicated  parallel  manipulator 
modules  with  more  than  one  degree-of-freedom  each.  Moreover,  the  whole  assembly 
is  not  anchored  to  a  base,  but  is  free  to  move  on  a  plane  and,  finally,  nonholonomic 
constraints  are  present,  in  addition  to  the  holonomic  ones.  However,  an  extension  of  the 
above  method,  allows  us  to  systematically  derive  the  kinematics  of  the  NVGT  assembly 
as  follows:  Using  the  Wei-Norman  representation  of  SE( 2),  we  express  the  shape  of  each 
module  as  a  product  of  the  one-parameter  subgroups  of  SE( 2).  Then,  the  configuration 
of  the  whole  assembly  can  be  expressed  as  a  product  of  such  one-parameter  subgroups. 
Using  the  notion  of  the  adjoint  action  of  SE( 2)  on  its  Lie  algebra,  we  determine,  in 
section  5.2.2,  how  the  motion  of  a  module  relates  to  the  motion  of  the  other  modules 
of  the  assembly.  We  also  express  the  nonholonomic  constraints  in  a  compact  form  that 
can  be  used  to  make  explicit  the  dependence  of  the  assembly  configuration  on  the  shape 
of  its  modules.  This  allows  us  to  characterize  the  dependence  of  the  global  motion  of 
the  assembly  on  the  shape  controls ,  namely  the  changes  in  the  shape  of  each  module, 
which  are  expressed  as  elements  of  the  Lie  algebra  of  SE( 2).  In  section  5.2.3,  we  consider 
the  implementation  of  each  module  as  a  planar  parallel  manipulator.  The  shape  of  each 
module  is  determined  by  the  lengths  of  the  legs  of  the  parallel  manipulator.  Prom  the  ve¬ 
locity  kinematics  of  the  parallel  manipulator  we  conclude  that  motion  planning  schemes 
for  the  NVGT  assembly  can  disregard  the  particular  details  of  the  implementation  of  the 
modules  and  only  consider  the  shape  of  each  module.  Thus,  instead  of  considering  the 
changes  in  leg  lengths  as  controls  for  the  NVGT  assembly,  we  can  use  the  corresponding 
shape  controls  of  each  module. 

In  section  5.3,  we  specialize  the  previous  discussion  to  the  3-node,  2-module  NVGT. 
Unlike  the  generic  £-node  case,  here  we  have  exactly  the  number  of  nonholonomic  con- 
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straints  that  we  need  in  order  to  determine  the  position  and  orientation  of  the  NVGT 
assembly  with  respect  to  the  world  coordinate  frame  based  on  a  sequence  of  shape  changes 
from  a  reference  shape.  As  a  result,  we  can  demonstrate  how  shape  changes  of  the  NVGT 
assembly  induce  a  global  snake-like  motion  due  to  the  nonholonomic  constraints.  We 
consider  the  motion  planning  problem  under  a  specific  shape  actuation  scheme,  where 
one  of  the  two  modules  is  responsible  for  the  motion  of  the  assembly  through  periodic 
changes  of  its  shape  and  the  other  module  is  responsible  for  steering.  We  demonstrate 
how  to  generate  primitive  “straight  line  motion”  and  “turning”  behaviors  and  we  show 
by  computer  simulations  how  to  synthesize  these  into  more  complex  ones,  like  avoidance 
of  obstacles. 

In  section  5.4,  we  consider  non-sequential  arrangements  of  NVGT  modules,  namely 
tree-like  arrangements  which  we  refer  to  as  5f?(2)-Spiders  and  ring-like  arrangements 
which  we  refer  to  as  5'i?(2)-Rings.  We  show  how  motion  control  for  such  systems  can 
be  reduced  to  the  corresponding  problem  for  the  2-module  NVGT.  In  section  5.4.3,  we 
consider  5JS(2)-Snake  assemblies  with  more  than  one  constraint  per  node.  Thus,  we 
relax  the  Codimension  1  Constraint  Hypothesis  of  the  previous  chapter. 

5.2  Kinematics  of  the  Nonholonomic  Variable  Geometry  Truss  (NVGT)  As¬ 
sembly 

In  section  5.2.1  \ve  discuss  the  nonholonomic  constraint  of  rolling-without-slipping 
and  in  section  5.2.2  we  apply  the  Wei-Norman  representation  of  curves  in  SE( 2)  to 
the  derivation  of  the  kinematics  of  the  £-node  NVGT.  In  section  5.2.3  we  consider  the 
implementation  of  a  module  of  the  NVGT  assembly  as  a  planar  parallel  manipulator. 

5.2.1  The  Nonholonomic  Constraint  of  Rolling-without-Slipping 

Consider  the  wheel  pair  assembly  of  fig.  5.2.1  consisting  of  two  idler  wheels  mounted 
on  an  axis,  so  that  they  can  rotate  freely  and  independently  of  each  other. 

Let  (f>r  (resp.  (pi  )  be  the  angle  of  the  right  (left)  wheel  of  the  pair  with  respect  to  the 
vertical.  Let  r  be  the  diameter  of  each  wheel  and  2  L  the  distance  between  the  wheels. 
Consider  a  coordinate  system  centered  midway  between  the  wheels  at  point  Oi„  with 
the  x-axis  along  the  axis  joining  them  and  pointing  towards  the  right  one  and  with  the 
z-axis  vertical  to  the  plane  supporting  the  wheel  assembly.  Let  the  wheel  assembly  move 
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'T- 

Fig.  5.2.1:  Wheel  pair  rolling-without-slipping 

with  respect  to  an  inertial  coordinate  system  centered  at  O0.  Let  g  G  G  —  SE( 2)  be  the 

(0  -w  Hi\ 

current  configuration  of  the  assembly  and  £  =  a;  0  S2  ]  G  Q  be  the  body  velocity 

\0  0  0  / 

of  the  assembly.  Then  g  =  g£.  It  is  a  simple  exercise  (c.f.  (Goldstein  [1980]))  to  derive 
the  nonholonomic  constraints  imposed  by  this  assembly  and  relate  the  motion  of  each 
wheel  to  the  body  velocity 

Proposition  5.2.1 

The  angular  velocity  of  each  wheel  is  related  to  the  body  velocity  of  the  wheel 
assembly  by: 

4>r  =  -(^2  +  Lu))  , 

[  (5-2.1) 

<fii  —  -(S2  —  Lu)  . 
r 

Proof 

Let  Pr  (Pi)  be  the  position  of  the  center  of  the  right  (left)  wheel  with  respect  to  the 
coordinate  system  centered  at  0\  and  pr  ( pi )  be  the  position  of  the  right  (left)  wheel 
with  respect  to  the  inertial  coordinate  system  at  O0.  Then  Pr  —  (L  0  1  )T  ,  = 
( —L  0  1)T,  while  pr  =  gPr  —  (x  +  LcosO  y  +  LsmQ  1  )T  and  pi  =  gPi  — 
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( x  -  L cos 9  y  +  Lsin  9  1 ) T  .  By  differentiating, 


(~u)L  sin  9  +  (Hi  cos  9  -  H2  sin  9)  \ 
u)L  cos  9  +  (Hi  sin  9  +  H2  cos  9) 

0  / 


(5.2.2) 


and 


Pi  =  gPi  =  g£Pi  = 


(ojL  sin  9  +  (Hi  cos  9  —  3 
-ljL  cos  9  +  (Hi  sin  9  + 
0 


Assuming  that  both  wheels  roll  without  slipping,  we  must  have: 


/  -  sin  9  \ 
pr  =  r4>r  cos  9  I 

V  0  / 


and 


Prom  (5.2.2)  and  (5.2.4): 


/  —  sin  9  \ 
Pi  =  r^>i  cos  9  I 

V  0  J 


Si  =  o, 

H2  =  rcj)r  -  Liv 


and  from  (5.2.3)  and  (5.2.5): 

3i=0, 

H2  =  rfa  +  Luj  . 


Thus: 


Hi  =  x  cos  9  +  y  sin  9  =  0  , 

2„ 

(pr  +  <f>l  =  -5 2  , 

r 

2  2  • 

<pr  —  (pi  —  —  Lio  —  -L9  . 


(5.2.3) 


(5.2.4) 


(5.2.5) 


(5.2.6) 

(5.2.7) 

(5.2.8) 


Observe  that  (5.2.8)  is  a  holonomic  constraint,  while  (5.2.6)  and  (5.2.7)  are  nonholonomic 
ones.  The  proof  of  this  appears  in  (Latombe  [1991]).  Since  u  —  ,  from  (5.2.8)  we  get: 

9(t)  =  9(0)  -  ~  (MO)  -  <Pi( 0))  +  ™(<Mt)  -  Mt))  (5-2.9) 
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Equations  (5.2.1)  follow  from  (5.2.7)  and  (5.2.8). 


5.2.2  The  f!-node,  (£  —  l)-module  Nonholonomic  Variable  Geometry  Truss 
(NVGT)  Assembly 

We  consider  a  chain  of  (£  —  1)  modules  of  the  type  shown  in  fig.  5.1.1  and  5.2.2. 
Each  module  consists  of  two  platforms  in  a  planar  parallel  manipulator  configuration 
with  one  pair  of  wheels  per  platform  and  with  each  wheel  rotating  independently  from 
the  other  around  its  axis,  both  forward  and  backwards.  Neither  wheel  pair  is  ac¬ 
tuated  and  we  assume  that  the  wheels  roll  without  slipping.  This  system  is  com¬ 
posed  by  £  planar  platforms,  thus  has  3£  degrees-of-freedom,  its  configuration  space 

is  Q  —  SE( 2)  x  ■  •  •  x  SE( 2)  and  it  is  subject  to  3(£  —  1)  holonomic  constraints  from  the 

' - - - ' 

i  times 

parallel  manipulator  legs  and  to  £  nonholonomic  constraints  from  the  rolling-without- 
slipping  wheel  motion.  The  configuration  of  the  assembly  can  be  determined  by  its  shape 
(which  is  an  element  of  the  shape  space  S  =  SE{2)  x  •  •  ■  x  SE{ 2)  )  and  by  the  position 

' - V - ' 

{l—  1)  times 

and  orientation  of  the  assembly  with  respect  to  the  world  coordinate  system  (which  is 
an  element  of  G  =  SE( 2)  )  .  Then  Q  =  G  x  S. 


Fig.  5.2.2:  The  (i,i  +  l)-th  module  of  the  NVGT  assembly 
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Consider  a  world  coordinate  system  centered  at  O 0  and  platform  coordinate  systems 
centered  at  Ou  i  =  1, . . .  ,£.  Let  g .  €  G  —  SE(2)  be  the  configuration  matrix  of  the  i-th 

l 

platform  with  respect  to  the  world  coordinate  system.  Define  £  e  Q  =  se( 2)  by: 

l 


g.=g.t,i  =  l,...,e.  (5.2.10) 

Prom  (4.3.2)  we  have  (with  a  slight  abuse  of  notation  we  denote  by  £*  G  JR  the  j-th 

j 

component  of£.,  i.e.  .A;  (£.)): 

i  J  i ' 


(5.2.11) 


3  =  1 


j= 1 


Also  define  the  vector  £*  of  the  components  of  £  6  Q  as  £*  =  (£*£*£l)T  = 

( 4({()  4(f()  )t  e 

Let  g .  G  G  be  the  configuration  matrix  of  the  (i  +  l)-th  platform  with  respect  to  the 
coordinate  system  of  the  j-th  platform.  Define  £.  +1  £  £  by: 


g  =  g  £  ,  for  i  =  1, . . . ,  i  —  1  and  1  <  j  <  i  +  1 

1  J,i+1  JM+1  ’  ’  ’  -J 


From  (4.3.2)  we  have: 


(5.2.12) 


k=l 


k= 1 


(5.2.13) 


Also  define  the  vector  £'J,'!+1  of  the  components  of  £  e  Q  as  P,l+1  = 

j.t+i 

(£j,1+i£j,i+1£i,i+i  )T  =  (  )  ^(£  )  ^(£  )  )T.  Observe  that  the  g  ’s 

v  1  2  ^3  ’  v  J,i+r  2vsj,»+ l’  dvsj,i+ 1J  ’  *3,1+ 1 

and  the  £  ’s  are  the  shape  variables  of  the  chain. 

3>*+ 1 

The  shape  of  the  NVGT  assembly  is  determined  by  {g,  ,  i  =  1, ...  ,1  —  1}.  The 

2  j  Z~\- 1 

velocities  {V^+i,  i  =  1, . . .  ,£  —  1}  are  called  s/iape  variations.  A  subset  of  them  are  the 
s/iape  controls. 

By  (4.3.3),  (4.3.21)  and  (4.3.23)  we  have  (note  the  definitions  of  Ri  and  T)): 


9.{t)  =  e 


Tl  (t)«4l  p72  (<)-^2 


3WA3  _  Ti  ^  ^ 


V  /  COS  4>i 

—  sin  4>i 

j  =  sin  (/>j 

COS 

y  V  0 

0 

1  / 

(5.2.14) 
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and 


9j,i+l 


(t)  = 


gYl  (*)-4lg72’' 


(  Rj,i+ 1  ^j,i+l  | 

Vo  W 


(cos  0J)t+i  -  sin 0jii+1  a;3)i+i  \ 
sin^j^+i  cos0jii+1  Vj,i+i  I 

0  0  1  J 


(5.2.15) 


where  the  7’s  and  the  corresponding  £’s  are  related  by  (4.3.14)  and  (4.3.15).  By  the 
system  kinematics  we  have: 


1+ 1 


9  9 

*i*i,i+ 1 


9  9  "9 

alal,2  *i,i+ 1 


t  =  l,...,*-l 


(5.2.16) 


and 


9.  =9.  9.  —  9 .  •••9. 

J,»  +  l  j,i  i,i  +  l  3,3  +  1  i,t  +  l 


for  i  =  1, 


-  1  and  !<;<*  +  !.  (5.2.17) 


Define  <?  .  =  /,  where  /  is  the  identity  in  G. 

3  >3 

Equations  (5.2.16)  with  (5.2.14)  and  (5.2.15)  can  be  seen  as  a  generalization  of  the 
“product-of-exponentials”  formula  of  (Brockett,  Stokes  &  Park  [1993])  for  kinematic 
chains  with  more  than  one  degree-of-freedom  per  joint. 

Lemma  5.2.2 

The  velocities  of  the  ( i  +  l)-th  node  depend  on  the  velocities  of  the  previous  nodes 
as  follows: 


£  ,  =  Adg-i  £.  + 

1+1  i,i+ 1  *  m+i 


Ad„- 1  •  •  ■  Ada- 1£  +  Ada-i  ■  ■  •  Ad0-i  £ 

9i,i+ 1  *1,2  1  9i,i+ 1  ®2,3  1.2 


H - h  Adg- 1  £.  +  £.  , . 

i,i+l  *-!.*  *.*+1 


The  velocities  of  the  (j,  i  +  l)-th  module  depend  on  those  of  modules  (j,  j  + 1),  ( j  + 1,  j  + 
2),  ...,(*,  i  +  1)  as  follows: 


£  =  Adg- 1  £.  .  +  £ 

3,1+ 1  »,*+!  ».*+! 


=  .AcL-i  •  •  •  Ada- 1  . 

i,i+l  j+l,j+2  id+1 


£.  .  ,  +  A<L-i  £ 

M+l  9j+2,j+3  J  +  1>J+2 

4 - +  Adg-i  £.  +  £.  . 

J.i+l  *-!.*  *>*+1 


(5.2.18) 
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Proof 


From  (5.2.10)  and  (5.2.16): 


9..  i  =9.9  ..  +  9.9.  ...  =9.i.9.  ...  +9.5.  ...f.  ... 
2+1  2  2,1+1  2  2,2+1  2  2  2,2+1  2  2,2  +  1  2,2  +  1 

=  9.9.  ...tor.1  A-9.  ...  +  £.  .,.] 

2  2,2  +  1  2,2  +  1  2  2,2+1  2,2+1 


=  g  [Adg-i  £.+£  ], 

,4-1  »  •  •  .  -  2  2,2  +  1 


(5.2.19) 


9 . , .  =  9.9.  •”9.  , 

i+l  1  1,2  z,i+l 

+  9.9.  •••9.  ...  +•’ 
1  1,2  2,2+1 

•  +  9.9.  •••9 .  . . . 

1  1,2  1,1+1 

=  g  [Mg-i  ■ 

1+1  i,i+l 

’  •  Adg—  1  £  +  Adg—1 

1,2  1  2,2  +  1 

Adg-i  £ 

2,3  1>2 

(5.2.20) 

+ - ^  ^.dg-l  £  +  £.  ] 

i.i+1  *.»+! 


Then  (5.2.18)  follows  from  those  and  (5.2.10).  The  equation  for  £  ^  is  derived  similarly. 


Corollary  5.2.3 

Equations  (5.2.18)  induce  the  following  relationships  between  the  positions  and  ve¬ 
locities  of  the  (i  +  l)-th  node  and  those  of  the  previous  nodes: 

ti+ 1  _  ti  I  (i,i+l 


q+1  =  -fx7r+1  +^cos+i+1  +£;sin7j’l+1  +  £ji+1 

^;+1  =  ^/+1  -  <e;  sm+t+l + e;  cos  7iii+1 + £;-i+1 . 


(5.2.21) 


ef+i  =  ^f+^’m, 

£+*+1  =  -£^73,i+1  + £+*  cos7*’i+1  +  £+*  sin7*’*+1  +  £’’i+1 
£+i+l  =  fj'-i72<+1  -  f  j*  sin7i’*+1  +  £+*  cos7j’i+1  +  £‘-i+1  . 
Moreover,  for  the  corresponding  Wei-Norman  parameters  we  have: 
7i+1  =  7i  +  7i’i+1  > 

l\+1  =7*cos7i’i+1  +7*sin7i’l+1  +7*>i+1  , 
7a+1  =  -72  sin7i,f+1  +  7s  cos  +l+1  +  7g’l+1 


(5.2.22) 


(5.2.23) 
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and 


7i’’+1  =  7i’*  +  7i’*+1  . 

7|'i+1  =  7"  cos7i'-+1  +  #  Sin7;',+1  +  7«+1  ,  (5.2.24) 

7f+1  =  -^‘sinyw+^cos-,;^1  +7r+1 . 


Proof 

Prom  (4.3.2),  (5.2.18),  (2.3.10)  and  (4.3.18),  observing  that  the  {Ai]  form  a  basis, 
we  get  equations  (5.2.21)  and  (5.2.22).  From  the  Wei-Norman  representation  of  elements 
of  SE( 2)  (equation  (4.3.20))  and  from  equation  (5.2.16),  we  get  equations  (5.2.23)  and 
(5.2.24). 


The  nonholonomic  constraint  of  rolling-without-slipping  on  the  wheels  of  each  plat¬ 
form  can  be  expressed,  using  (4.3.23),  as: 

€=  =  ii  cos  fa +&  sin  fa  =  0  ,  i  =  1,  (5.2.25) 

2  * 

Define  the  composite  velocity  vector  (  of  the  NVGT  assembly  as  the  components  of  the 
velocity  £  of  the  first  platform  and  those  of  the  shape  variations  of  all  the  modules  of 
the  assembly: 


cd=  u1u1’2i-"ie*-1>/)T 

L  Z  O  i.  1  Z  1 


T 


Theorem  5.2.4 

The  l  nonholonomic  constraints  (5.2.25)  can  be  written  in  matrix  form  as: 


(5.2.26) 
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where  A  is  a  function  of  only  the  shape  of  the  NVGT  assembly  {g  ,  i  =  1,  -  ■  •  1} 

i,i+l 

and  is  an  £  x  M  block  lower  triangular  matrix  of  maximal  rank  of  the  form: 


^(^1,2’  !^-l,P 


/  *1,1 

0 

0 

0 

0 

0 

°  \ 

*1,2 

*2,2 

0 

0 

0 

0 

0 

: 

0 

0 

0 

*l,t+l 

*2,1+1 

*1,1+1 

*1+1, J+l  •  •  • 

0 

0 

I 

0 

V  *1,* 

*2  ,i 

*i,e 

*1+1,  i 

*e,e  J 

with  the  /c-th  block  of  the  ( i  +  l)-th  line,  for  i  —  1, ...  ,£  —  1  defined  as: 

*k,i+ 1  =  ( A(Ad9-x  «4.i)  AziAdg-i  ^4.2)  A^{Adg-i  ^3) 

\  1  fc,i+ 1  k,i+ 1 


—  I  A-2 {-Adg-i  •  •  •  Adg-i  Ai)  A2 {Adg-i  •  •  •  Adg-i  A2) 

i,i+ 1  fc,/c  + 1  i,i+l  fc,/c+l 


A2^Adg— 1  •  •  •  t4c^o-i  ^4.3) 

M+l 


=  (  —  73fe’l+1  cos7^’*+1  sin7^’I+1 ),  for  1  <  k  <  i  +  1  , 

*k,k  =  (  A^iAdg-i  .Ai)  A\{Adg-.  A2)  A\{Adg-i  A3)  ) 

V  k,k  k,k  k,k  J 


=  (010), 

*k,i+ 1  =  (000), 


for  k  =  i  +  1  , 
for  i  +  1  <  k  <  t . 


(5.2.27) 


(5.2.28) 


Proof 

This  result  follows  from  Theorem  4.2.1. 


Since  A  has  always  maximal  rank  £,  its  null  space  Af{A)  has  dimension  m  =  3 1—1  = 
2  l. 

Corollary  5.2.5 

Assume  that  the  velocities  (  can  be  reordered,  so  that  the  matrix  A  is  partitioned 
as  A  =  ( Ai  A2 ) ,  with  A\  an  i  x  m  matrix  and  A2  a  locally  invertible  i  x  £  matrix. 
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Let  the  corresponding  partition  of  the  velocity  vector  be  (  =  j  ,  with  Ci  £  and 
( 2  £  IRe-  Then,  there  exists  an  3 £  x  m  matrix  B  such  that: 


c  = 


=  5  Ci- 


(5.2.29) 


Proof 

Follows  from  Corollary  4.2.2.  The  matrix  B  = 


works. 


The  physical  meaning  of  this  result  is  that  the  global  motion  of  the  NVGT  assembly 
depends  only  on  the  current  shape  of  the  assembly  and  on  the  variations  of  this  shape. 
Direct  actuation  of  the  wheels  (as  in  a  car)  is  not  necessary  for  propulsion  of  this  system. 

Notice  that,  since  A  depends  only  on  the  shape,  so  does  A2.  In  addition,  the  choice 
of  the  locally  invertible  matrix  A2  is  dictated  by  the  choice  of  the  splitting  of  C  into 
Ci  and  C2-  For  a  particular  choice,  as  the  shape  is  altered,  A2  will  become  singular 
at  certain  shapes.  However,  a  different  splitting  of  C  may  provide  a  nonsingular  A2. 
If  this  is  not  the  case,  the  corresponding  configurations  of  the  NVGT  assembly  shall 
be  referred  to  as  nonholonomic  singularities.  Those  singularities  are  not  removable 
by  merely  choosing  alternative  splittings  of  C  or  alternative  parametrizations  of  the 
configuration  space  (e.g.  how  we  assign  orderings  to  the  kinematic  chain  at  hand). 
In  these  configurations,  the  system  kinematics,  together  with  the  shape  control,  may 
not  provide  sufficient  information  to  determine  the  motion  of  NVGT  assembly  and  the 
dynamics  of  the  system  may  have  to  be  used  in  the  form  of  a  ballistic  motion  through 
the  nonholonomic  singularity. 

Remarks  5.2.6 

1)  Unlike  previous  work  on  nonholonomic  motion  planning,  in  our  case  the  Ci’s  of 
equation  (5.2.29)  do  not  correspond  directly  to  the  controls  of  the  system  and,  thus, 
are  not  at  our  disposal  to  alter  at  will.  Our  real  controls  are  the  leg  velocities  a  of  the 
parallel  manipulator  modules  (see  next  section).  However,  as  we  will  see  in  the  next 
section,  off  the  kinematic  singularities  of  the  parallel  manipulators,  the  shape  controls 
can  easily  determine  the  corresponding  leg  velocities.  Therefore,  in  order  to  simplify  the 
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discussion  of  motion  planning,  once  the  partitioning  of  the  velocity  vector  £  as  (  ^  |  is 

VC2 

done  in  such  a  way  that  all  the  (i’s  are  controllable  from  the  <r’s,  we  will  disregard  the 
particulars  of  the  implementation  of  the  modules  and  only  consider  the  shape  controls. 
Note  that  when  we  have  only  one  module,  i.e.  I  =  2,  such  a  partitioning  of  the  velocity 
vector  cannot  be  done. 


2)  The  3(£  —  1)  holonomic  constraints  imposed  by  the  legs  of  the  modules  of  the 

{£—  l)-module  NVGT  assembly  determine  the  shape  g  ,g  , . . . ,  g  of  the  assembly. 

1,2  2)3  £■ — 1  )^ 

In  order  to  determine  completely  its  configuration,  we  also  need  to  specify  the  position 
and  orientation  of  the  assembly  with  respect  to  the  world  coordinate  frame,  given  by  an 
element  ( g  in  this  case)  of  G  =  SE( 2).  Thus,  we  need  3  more  constraints.  These  come 
from  the  £  nonholonomic  constraints  provided  by  the  platform  wheels.  Consider  now 
some  special  cases:  i)  If  we  have  a  one-module  NVGT,  i.e.  if  £  =  2,  we  have  3  holonomic 
and  2  nonholonomic  constraints,  but  we  need  to  determine  6  degrees-of-freedom,  thus 
we  do  not  have  enough  kinematic  constraints  to  determine  the  motion  of  the  assembly. 
We  either  have  to  consider  its  dynamics  or  we  need  to  impose  additional  constraints  (e.g. 
unidirectional  wheel  motion).  The  first  alternative  is  particularly  interesting  and  gives 
rise  to  the  “Roller  Racer” system  that  we  consider  in  chapter  6.  ii)  If  we  have  a  two- 
module  NVGT,  i.e.  if  £  =  3,  we  have  6  holonomic  and  3  nonholonomic  constraints  and 
we  need  to  determine  9  degrees-of-freedom,  thus  we  have  exactly  the  required  number 
of  constraints,  in)  If  we  have  an  NVGT  assembly  with  more  than  two-modules,  i.e. 
if  £  >  3,  we  have  3  {£  —  1)  holonomic  and  £  nonholonomic  constraints  and  we  need  to 
determine  3£  degrees-of-freedom,  thus  we  have  3(£  —  1)  +1  —  31  —  l  —  3  extra  constraints 
that  have  to  be  satisfied.  Therefore,  from  the  3(£  —  1)  shape  velocity  components  in  £, 
only  3(£  —  1)  —  (£  —  3)  —  2£  can  be  determined  independently  and  will  be  elements  of 
the  vector  G  in  equation  (5.2.29).  The  remaining  £  —  3  shape  velocity  components  will 
be  determined  by  the  £  —  3  extra  constraints,  i.e.  they  will  be  elements  of  the  vector  (2, 
together  with  the  velocities  that  characterize  the  global  motion  of  the  assembly  (here  they 
are  the  ^’s).  Observe  that  if  there  exists  non-negative  integer  k  such  that  £  —  3  =  3 k, 
we  can  choose  £  —  k  of  the  £  modules  and  alter  their  shape  at  will,  while  the  shape  of  the 
remaining  k  modules  will  be  determined  by  the  extra  constraints.  In  brief,  for  £  >  3,  the 
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problem  is  over-constrained. 

3)  The  2-module  NVGT  is  “canonical”  in  the  following  sense:  Suppose  that  we 
change  the  (£  —  l)-module  NVGT  architecture  (for  l  >  3)  so  that  we  have  wheels  on 
only  3  platforms  and  castors  on  the  remaining  l  —  3  platforms.  Then  we  have  only 
3  nonholonomic  constraints  (instead  of  £).  Such  a  system  has  exactly  the  number  of 
constraints  needed  to  determine  its  motion,  which,  in  our  original  NVGT  assembly  is 
the  case  only  for  l  =  2.  The  motion  planning  strategies  that  will  be  determined  for 
the  2-module  NVGT  will  work  also  for  this  {t  —  l)-module  assembly.  Moreover,  this 
(£  —  l)-module  assembly  possesses  M  —  3  degrees-of-freedom  more  than  the  2-module 
NVGT  and  those  can  be  exploited  for  other  purposes  (e.g.  for  motion  in  a  constrained 
environment,  or  for  generating  a  richer  repertoire  of  motions  of  the  type  described  in 
(Chirikjian  &  Burdick  [1991])). 


5.2.3  Implementation  of  an  NVGT  module  as  a  Parallel  Manipulator 

Consider  the  ( i,i  +  l)-th  module  of  the  NVGT  assembly,  implemented  as  a  planar 
parallel  manipulator  (c.f.  section  2.5).  This  consists  of  the  i- th  and  (i  +  l)-th  platforms, 
which  are  connected  by  three  legs  of  variable  length.  One  possible  architecture  is  shown 
in  fig.  5.1.1. 

In  previous  sections  we  saw  that  the  shape  of  the  (i,  i  +  l)-th  module  is  determined 
by  g.  .  E  SE( 2).  Let  pPf,  for  k  =  i,  i  +  1,  j  =  1, 2,  3  be  the  position  of  the  j-th  joint 

of  the  fc-th  platform  with  respect  to  the  coordinate  system  of  this  platform.  Consider  the 
following  quantities,  defined  with  respect  to  the  coordinate  system  of  the  i-th  platform: 
the  position  Pj+1  of  the  (i  4-  l)-th  platform,  the  vector  S')  and  the  length  cr*  of  the  j-th 
leg  of  the  module.  Let  a 1  be  the  vector  of  the  leg  lengths  of  the  (i,i  +  l)-th  module,  i.e. 
a  =  {a1  a\  a\)  . 

The  inverse  kinematic  map  P~1  :  SE( 2)  ->  specifies  the  leg  lengths  of  the 
module  as  functions  of  its  shape  g  ,  .  We  can  easily  see  that  for  the  j-th  leg: 


(5.2.30) 
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where  g  ,+i  =  (■R,’0i+1  Tl,’+1 ).  Then  the  leg  lengths  of  the  module  are  given  by 

=  ll^lla  =  y/<  3>  fori  =  1,2,3,  (5-2.31) 

where  <  ,  >n  is  the  inner  product  in  lRn  and  || .  ||n  is  the  corresponding  norm.  Then  the 
inverse  kinematic  map  is: 

T~\g  (5.2.32) 

M+l 

Applying  Theorem  2.5.3  in  this  case,  we  get  the  following  results. 

Proposition  5.2.7  (Velocity  Kinematics) 

The  body  velocities  of  the  i-th  module  and  the  changes  of  length  of  its  legs 

a1  c=  ol2  a\)T  are  related  by: 


E(aV  =  J(9i<i+1)e’i+1  - 


(5.2.33) 


where  £(<7*)  =  diag{o\ ,  a 2,  al}  and  J(g.  .  )  = 

i,i+l 


Proof 


sf  Ri>i+1PPi+1  I  sfRi<i+1 


def 


From  (5.2.31):  (cr *-)2  =<  5],  S']  >3  .  Differentiating  both  sides  and  defining  fi  = 
f;l+1  and  S  d=  (*2  j  ,  we  get: 


=<  S),S)  >3=<  Si,f?t)i+1(H  +  OPPi+1)  >3 


=<  r^+1ts%  s  +  ppj+1  n  >3=<  ppj+1f?M+iTs!.,  n  >3  +  <  Ri,i+1TS),  s  >3 , 


where  was  defined  in  (5.2.15).  Then  (5.2.33)  follows. 


Configurations  where  J(g)  or  £(<7(5))  is  singular  are  called  kinematic  singularities. 
Those  have  nothing  to  do  with  the  nonholonomic  singularities  of  the  NVGT  assembly 
defined  in  the  previous  section. 
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Corollary  5.2.8 


Suppose  the  configuration  g .  €  SE( 2)  of  the  (i,i  +  l)-th  module  is  not  a  kine¬ 

matic  singularity  and  that  the  corresponding  leg  lengths  are  a 1 .  Then: 


i,i+ 1 


(5.2.34) 


and 


(5.2.35) 


Observe  that  after  we  specify  the  shape  variations  £*’l+1,  the  corresponding  leg 
length  changes  can  be  easily  determined  from  (5.2.35).  Therefore,  our  discussion  of 
the  motion  planning  problem  can  disregard,  without  loss  of  generality,  the  particular 
implementation  details  of  the  modules  and  consider  only  the  shape  variations  of  the 
modules. 

5.3  The  3-node,  2-module  NVGT  Assembly 

In  section  5.3.1  we  consider  the  kinematics  of  the  3-node,  2- module  NVGT  (fig. 
5.3.1)  as  a  special  case  of  the  kinematics  of  the  £-node  NVGT.  We  show  that  the  velocity 
vector  (  can  be  partitioned  in  two  parts,  one  of  which  ((]  )  constitutes  the  independent 
shape  controls  and  the  other  (£2)  being  velocities  dependent  on  the  shape  variables,  which 
characterize  the  motion  of  the  assembly  with  respect  to  the  world  coordinate  system.  In 
section  5.3.2  we  examine  the  motion  planning  problem  for  the  2-module  NVGT  assembly. 
We  show  that  shape  actuation  strategies,  where  the  shape  of  one  module  is  kept  fixed 
and  the  shape  of  other  is  varied  periodically,  induce  a  rotation  of  the  NVGT  assembly 
around  the  instantaneous  center  of  rotation  of  the  first  module.  If  the  platforms  of  the 
fixed  shape  module  are  parallel,  the  induced  motion  is  a  translation  along  a  direction 
perpendicular  to  the  platforms.  We  allow  the  shape  of  the  second  module  to  describe 
a  closed  path  in  shape  space  and  show  that  a  net  displacement  of  the  NVGT  assembly 
with  respect  to  the  world  coordinate  system  is  induced  after  each  traversal  of  the  shape 
space  path. 
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Fig.  5.3.1:  Two-module  NVGT  assembly 


5.3.1  Kinematics  of  the  2-module  NVGT 

In  the  assembly  of  fig.  5.3.1,  we  consider  a  chain  of  two  NVGT  modules.  This  system 
has  Zi  =  9  degrees-of-freedom,  its  configuration  space  is  Q  =  G  x  S,  where  G  =  SE( 2) 
and  the  shape  space  is  S  =  SE( 2)  x  SE( 2)  and  it  is  subject  to  3 (£  -  1)  =  6  holonomic 
constraints  from  the  parallel  manipulator  legs  and  to  i  =  3  nonholonomic  constraints 
from  the  rolling-without-slipping  assumption  on  the  wheels.  From  the  system  kinematics 
we  have  (specializing  the  results  of  section  5.2): 


g  =  g  g  , 


^3  ^2^2,3  ^1^1, 2^2, 3  ’ 


g  —  g  g 

^13  al,2a2,3 


(5.3.1) 
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Prom  (5.2.18)  we  get  for  the  corresponding  velocities: 


*2  =  ^2*1  +  *1.2’ 


L  =  Adg-i  L+L  =  Adg-1  Adg- 1 1  +  Adg- 1  £  +  £  , 

3  2,3  2  2-3  2,3  1,2  1  2,3  1>2  2>3 

£  =  Adg- +£  . 

1,3  92.3  1.2  2,3 


(5.3.2) 


The  nonholonomic  constraint  of  rolling-without-slipping  on  the  wheels  of  each  platform 
can  be  expressed  using  (5.2.25)  as: 

e*=4(e.)  =  iicos^  +  yisin^=0,  *  =  1,2,3.  (5.3.3) 

2  ^ 

The  3  nonholonomic  constraints  can  be  put  in  the  matrix  form  of  (5.2.26): 

AK2'92,JS=0'  (5‘3'4) 


where  (  =  £1,2  .  The  matrix  A  is  a  function  of  only  the  shape  variables  g  ,  g  of 


1,2'  2,3 


the  chain  and  is  a  block  lower  triangular  matrix  of  the  form: 

/  *1,1  0  0  \ 

Ablt2'92,3>  =  *1,2  ^  0 

\  *1,3  *2,3  *3,3/ 


0  1  0 

A^{Adg- 1  Ai)  A^{Adg-i  A?)  A2{Adg-i  .A3) 

1.2  1,2  1,2 

A^{Adg-i  A\)  Ab2(Adg-i  A2)  Ai2{Adg-i  Ai) 

1.3  1,3  1,3 

0  0  0  0  0  0 

0  1  0  0  0  0 

A\{Adg-xAx)  A\{Adg-iA2)  A^(Adg-iAz)  0  1  0 


0  0  0  0 

-7g’2  COS7J’2  sin7|’2  0  1  0  0  0  0 

1,3  1,3  •  1,3  2,3  2,3  •  2,3  n  1  n 

-73  cos71  sm71  -73  cos71  sm71’  0  10 


(5.3.5) 


Equation  (5.3.4)  can  be  put  in  the  form  (5.2.29)  by  partitioning  (  as  £  =  ^  with 

*-(£)  and  (2  =  e  (5.3.6) 
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and  by  partitioning  A  as  (Ai  A2 )  with 


/  0  0  0  0  0  0 

A1(g  )  =  0  1  0  0  0  0 

2)3  \  A^Adg-i  Ai)  A^Ada-i  A2)  Ab2(AdQ-i  A3)  0  1  0 

X  2,3  2,3  2,3 

/  0  0  0  0  0  0\ 

=  0  1  0  0  0  0 

y  — T3'3  COS72’3  sin7i’3  0  1  0/ 


and 


/  0  1 

A2{Ad„-i  Ai)  A2(Ad„-i  A2) 
1,2  1,2 

A2(Adg~iAi)  A2(Adg-iA2) 
\  1,3  1,3 


°  \ 

A2(Adg-i  ^.3) 

1,2 

A2(Adg-i  A^) 

1,3  / 


/  0  1  0  \ 

I  -73 ’2  COS7J-2  sin7j’2  ] 

\-73  cos7i  sin7^’3  J 


(5.3.7) 


(5.3.8) 


Then  the  velocity  of  the  2-module  NVGT  assembly  with  respect  to  the  world  coordinate 
system,  as  it  is  characterized  by  can  be  expressed  as  a  function  of  only  the  shape 
variables  of  the  assembly: 


^  =  -A2H9h2,92t3)M92t3) 


e1*2 

£2,3 


(5.3.9) 


Proposition  5.3.1 

The  nonholonomic  singularities  of  the  2-module  NVGT  assembly  are  configurations 
where  all  3  axes  of  the  platforms  of  the  assembly  either  intersect  at  one  point  or  are 
parallel. 

Proof 

The  nonholonomic  singularities  of  the  system  are  specified  by  considering  the  determi¬ 
nant  of  the  matrix  A2  : 


det(A2)  =  A\{Adg~,  AM^Adg- 1  ^3)  -  A\{Adq^  Ai)A\(Adq-i  .43) 

1,3  1,2  1,2  1,3 

1,3  •  1,2  ,  1,2  .  1,3 

= -7s  sm71  +73  sm7i 

2,3  2,3  •  1,2 

=  3/1,2  sm71’  -73’  sm7l  . 


(5.3.10) 
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Observe  that  when  sin7*’2  /  0  and  sin7*’3  /  0  : 


det{A2)  =  sin 7I’2  sin  7^ 


1,3 


731,3 

1,3 

sin  7X 


1,2 

73 

1,2 

sin  7X 


(5.3.11) 


=  sin  7f2  sin  yf’3  [Ax0li3  -  Aa;0l,2]  , 


where  A xo,  ,  =  — — ,  j  —  2, 3,  is  the  distance  of  the  intersection  Oi  ,  of  the  axis 

sin71’J 

of  platform  1  with  the  axis  of  platform  j  from  the  point  0\,  as  shown  in  fig.  5.3.2.  It 
will  be  shown  in  the  next  section  that  the  point  0\tj  coincides  with  the  Instantaneous 
Center  of  Rotation  (ICR)  of  the  module  composed  of  the  platforms  1  and  j.  Then,  the 
matrix  A 2  is  singular  whenever  the  points  Oi,2  and  Oii3  coincide  (the  point  Oi,2,3  may 
be  at  infinity  as  in  fig.  5. 3. 3. a). 


Fig.  5.3.2:  Instantaneous  Center  of  Rotation  of  the  (i,  j)-th  Module 

It  can  be  shown  that  nonholonomic  singularities  are  not  removable  by  reparametriz¬ 
ing  TqQ,  i.e.  if  the  global  motion  of  the  2-module  NVGT  is  characterized  by  £  or  £  , 
as  opposed  to  £  ,  the  equations  corresponding  to  (5.3.4)  and  (5.3.9)  will  provide  us  with 
the  same  set  of  nonholonomic  singularities  that  appear  in  the  above  Proposition. 

Even  in  the  case  the  system  is  at  a  nonholonomic  singularity,  the  3  nonholonomic 
constraints  remain  independent  (c.f.  equation  (5.3.5),  where  rank(A)  =  3),  but,  since 
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(a)  (b) 

Fig.  5.3.3:  Nonholonomic  Singularities  for  2-module  NVGT  assembly 


the  platforms  have  a  common  instantaneous  center  of  rotation,  equation  (5.3.4)  cannot 
be  recast  in  the  form  of  (5.3.9).  Therefore,  motion  with  respect  to  point  2,3  cannot  be 
controlled  by  the  system  shape  variables  alone  and  the  dynamics  of  the  system  ought  to 
be  considered.  This  is  analogous  to  what  practicing  engineers  refer  to  as  loss  of  control 
authority. 


a-  0 


(X  »  1 


Fig.  5.3.4:  Nonholonomic  Singularity  Surfaces 


In  the  motion  control  strategies  that  we  will  consider  in  the  next  section,  the  (1, 2)- 
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module  will  be  used  for  steering,  therefore  its  shape  will  remain  constant  for  relatively 
large  periods  of  time.  We  are  interested  in  finding  out  what  the  locus  of  nonholonomic 
singularities  (as  a  function  of  the  shape  of  the  (2, 3)-module)  is  under  this  condition. 

Prom  (5.3.10),  the  condition  detA2  =  0  becomes  q2’3  =  cc(7i ’2)  sin q2,3  ,  with  =f 

sinffi*  W^en  sin7i’2  7^  0  or  sinq2,3  =  0,  when  sin711,2  =  0.  The  first  relationship  is  shown 

in  fig.  5.3.4,  where  the  usual  geometric  picture  of  SE(2)  as  a  “thick”  cylinder  is  used 
to  visualize  the  loci  of  nonholonomic  singularities  for  various  values  of  a.  Assuming  that 
the  71-axis  runs  around  the  cylinder,  the  72-axis  runs  along  its  length  and  the  73-axis 
runs  along  its  radial  direction,  we  consider  a  cut  of  this  cylinder  vertical  to  the  q2,3-axis. 
Observe  that  in  either  one  of  the  above  cases,  the  locus  splits  SE( 2)  in  two  disconnected 
parts.  If  we  want  to  go  from  one  part  to  the  other  we  have  to  go  through  this  manifold  of 
nonholonomic  singularities.  Crossing  this  locus  in  the  most  appropriate  way  may  require 
path  planning  of  the  type  presented  in  chapter  3,  in  order  to  move  transversally  to  this 
manifold  and  allow  the  dynamics  of  the  system  to  carry  the  system  from  one  part  of 
SE( 2)  to  the  other. 

Prom  (5.3.8): 


A-i  =  _L_ 

2  det(A2) 


vii.1  fl 

1,2  •  1,3  1,3  •  1,2 

73  sin7i  -73  sm7i 
1,2  1,3  ,  1,3  1,2 

v-7s  cos  7i  +73  cos  7i 


—  sin7^’3 


-7s1’3 


(5.3.12) 


Then,  from  (5.3.9)  we  get: 


-1 


det(A2 ) 


1,2  2,3 
—  sin 7^  73’ 

0 

1,2  2,3 
-73  73 


—  sinq*’3  +  sinq*’2  cosq2,3 
0 

1,3  .  1,2  2,3 

-73  +  73  cos  7i 


sinq*’2  sinq2’3 


0 

0 

0 


1.2  •  2,3 

73  sm7i 

sin  7J  ’2 


0 

1,2 

73 


(5.3.13) 

Observe  that  the  partitioning  of  (  in  equation  (5.3.6)  is  the  only  one  that  assigns  to 
Ci  shape  controls  which  can  be  affected  by  leg  length  changes  in  the  parallel  manipulator 
modules.  Moreover,  it  assigns  to  £2  the  velocities  that  characterize  the  global  motion  of 
the  NVGT  assembly  with  respect  to  the  world  coordinate  system. 
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Proposition  5.3.2  (Controllability) 

The  2-module  NVGT  assembly  is  controllable  from  any  initial  position  g\  £  G  = 
SE( 2)  to  any  final  one  g\  £  G. 


Proof 

The  left-invariant  system  =  g^x  with  ^  given  by  (5.3.13)  as  £  =  u\A\  +  U3A3 
is  such  that  [^4i ,  ^l3]  =  -A2,  thus  sp{^4i,  ^t3,  [^4i,  ^.3]}  =  Q ■  Thus,  it  is  controllable 
from  any  initial  position  g\  £  G  =  SE( 2)  to  any  final  one  gi  £  G,  whenever  ui  and  u3 


are  independent  controls.  For  a  generic  set  of  shape  controls 


1.2 

2.3 


this  will  happen 


whenever  the  1st  and  3rd  rows  of  the  matrix  A^Ai  in  (5.3.13)  are  linearly  independent. 
By  checking  all  the  2x2  determinants  generated  by  those  rows,  we  easily  see  that  this 
is  always  true  away  from  the  nonholonomic  singularities. 


5.3.2  Motion  Control  and  Planning  for  the  2-module  NVGT 

There  are  several  possible  actuation  strategies  for  the  2-module  NVGT.  We  will 
consider  a  simple  one  where  the  first  module  “steers”  the  system,  while  the  second 
provides  the  translation  mechanism  through  periodic  variations  of  its  shape  parameters. 


The  Motion  Control  scheme  that  we  consider  here  is  open-loop  and  is  based  on 
periodic  oscillations  of  the  shape  of  the  assembly.  The  model  for  the  2-module  NVGT 
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assembly  is  summarized  in  fig.  5.3.5.  When  we  specify  the  shape  controls  ^  ^  and  ^ 
the  corresponding  shape  trajectories  ^(t)  and  923{t)  are  given  by  the  Wei-Norman 

procedure  (the  “W-N” block  in  the  above  figure,  which  refers  to  equations  (4.3.3),  (4.3.14) 
and  (4.3.15)).  The  nonholonomic  constraints  are  then  used  to  calculate  the  instantaneous 
motion  of  the  whole  assembly  with  respect  to  the  world  coordinate  system,  as  it  is 
described  by  (equation  (5.3.13)).  Finally  the  Wei-Norman  procedure  allows  us  to 
determine  the  corresponding  system  trajectory  g  .  This  whole  process  is  detailed  below. 

From  (5.3.13)  we  observe  that  the  motion  of  the  system  is  determined  completely, 
at  least  away  from  the  nonholonomic  singularities,  by  the  shape  controls  vectors  £1,2  and 
£2,3.  Here  we  will  consider  the  special  case  of  motions  that  are  generated  by  keeping  the 
shape  of  the  first  module  fixed,  i.e.  £1,2  =  0,  and  vary  the  shape  controls  £2,3  of  the 
second  module  periodically.  Then  from  (4.3.14): 


7^2  _ 


(tr\ 

(U2 


2 

1,2 


VC7 


=  o 


(5.3.14) 


Thus  from  (4.3.15): 


7i’2  =  7i’2(0)  , 


1,2 

72  =  72  (0)  , 


1,2 

73 


731,2(0) 


(5.3.15) 


Also  : 


7 


2,3 


'V2’3\ 

\  1  =  1 

(  1 

0 

°\ 

732’3 

1 

0  ) 

V-722’3 

0 

l) 

U,2’3 

(5.3.16) 
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Then  from  (4.3.15): 


2,3 

7i 


t 

’3(t)  =  7i’3(0)  +  J  ^’3(r)dr  , 


t  t 

72,3W  =72,3(0)cos  ^  J  ^’3(a)d^  +  72’3(0)  sin  ^  J  ^3(a)dc^J 

n  n 


+ 


t  t  t  t 

J  ^’3(r)cos(/  t*fi(*r)dajdT  +  J  £g’3(T)  sin  ^  J  Z*'3(<r)do\  dr  , 

n  T  (\  *r 


t  t 

732’3«)  =  -72,3(°)sin  (^J  £2’V)<^  +73,3(°)cos  (^J  C^,3(cr) dcr 

0  0 

t  t  t  t 

-  J £2,3(T)sin  (y  Z2i’3(a)doSJdT  +  J ^3(T)cos(/ ^2’3(cr)da^  dr  . 

Or  0  t 


(5.3.17) 


Prom  (5.2.24),  (5.3.10)  and  (5.3.14): 

det{A2)  =  -731’3sin711’2(0)  +731’2(0)sin711,3 

=  -  7a2’3  -  721,2(0)  sin 72,3  +  73’2(0)  cos7l2’3]  sin7l1’2(0) 

+  731,2(0)  sin(7i’2(0)  +  72,3) 

=  yll2(0)  sin72’3  -  7I’3  sin711’2(0)  . 

From  (5.3.13): 

.1,2 


(5.3.18) 


«*  = 


(*W 

A 


_  /  0  sin7l1’2(0)  O' 

0  0  0 

0  73’2(0)  0. 


det(A2) 


/e,3\ 

£2,3 

vev 


— £2>3  /  sin7^’2(0) 

- - —  (  0 

det{M)  V  731,2(0) 


(5.3.19) 

Observe  that  £*  =  0,  as  is  expected  from  the  nonholonomic  constraints.  Then  from 
(4.3.14)  we  have 


7 


(5.3.20) 
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and  from  (4.3.15): 


t 

71  W  =  7i(0)  +  J  £j(r) dr  , 

o 

t  t 

72  W  =  72  (°) cos  (  J  ^V)^  +  73  (0)  sin  ^  J  ^(a)da 


t  t 

+  J Q(t)  -n(/  £|(cr )dcr^dT  , 


t  t 

73  W  =  -72(0)  sin  ^  J ^(^da^j  +73(0)  cos  ^  J Q(cr)do) 


+ 


Z  Z 

J ^)cos(/ . 

0  T 

(5.3.21) 


Theorem  5.3.3 


i)  If  £1,2  =  0  and  7J’2  =  0,  the  2-module  NVGT  instantaneously  translates  along 
an  axis  perpendicular  to  platforms  1  and  2. 

ii)  If  £1,z  =  0  and  7J’2  /  0,  the  2-module  NVGT  instantaneously  rotates  around 
the  intersection  of  the  axes  of  platforms  1  and  2. 


Proof 

i)  Let  7 1’2  —  0.  From  (5.3.18)  and  (5.3.19): 


C  =  0 , 


«‘=°. 


sin  72,3 


(5.3.22) 
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Prom  (5.3.16): 


From  (4.3.23): 


7i  =  0  1 

72=0 
73  =  £ 


7i(*)  =  7i  (0)  , 
7a(*)  =  7a(0)  . 


73  W  =  73 (0)  +  J ^{T)dr  . 
o 


f  xA  /  —  sin7f  (0)  \ 

I  m  =  cos  7i1(0) 

\4>i)  3V  o  J 


(5.3.23) 


(5.3.24) 


Thus,  platform  1  translates  along  the  axis  that  passes  through  the  point  0\  and  is 
parallel  to  the  vector  (—sin  7^(0) ,  cos 7^(0)),  i.e.  perpendicular  to  the  platform.  This  is 
a  constant  vector,  thus  the  point  Oi  traces  a  straight  line.  Moreover,  since  </> i  is  constant, 
the  whole  platform  translates  along  this  line. 

ii)  Let  7^’2(t)  =  7i ’2 (0)  ^  0.  The  instantaneous  center  of  rotation  (ICR)  of  the 
velocity  distribution  of  equation  (5.3.19)  (Bottema  &  Roth  [1979])  can  be  proven  to 
be  the  point  Oi,2,  where  the  axes  of  platform  1  and  2  intersect.  To  see  this,  assume 
that  the  ICR  has  coordinates  ( xlP,y\, )  with  respect  to  the  fixed  world  coordinate  system 
and  (xp2,yp2)  with  respect  to  the  moving  coordinate  system  of  platform  1.  Define 
xp  =  ( Xp  ylP  1)T  and  Xp  =  (xp2  j/p2  1)T.  By  its  definition,  Xp  —  0.  Then: 


XP  =  9lXP  =>  xp  —  g^Xp  +  g^Xp  =  g^_Xp  .  (5.3.25) 

The  ICR  is  defined  as  the  point  where  xp  =  0.  From  this  and  (5.3.25)  we  get: 


tXP  =  0  ^  a:p2  = 


731’2(0) 

sin7i1,2(0) 


Vp2  =  0  • 


(5.3.26) 


From  fig.  5.3.2,  it  is  easy  to  see  that  this  is  point  Oi,2,  the  intersection  of  the  axes  of 
platform  1  and  2. 

Furthermore,  it  is  possible  to  show  that  the  ICR  is  constant,  not  only  with  respect  to 
the  coordinate  system  of  platform  1,  as  is  immediately  evident  from  the  expressions  for 
(ip2,  l/p2)  in  (5.3.26),  but  also  with  respect  to  the  world  coordinate  system.  Therefore, 
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the  motion  of  the  system,  is  indeed  a  rotation  around  this  point.  To  see  this,  consider 
the  vector  xp  as  a  function  of  time  t  and  expand  in  Taylor  series  around  a  fixed  time 
instant  to-  Then,  defining  At  =  t  —  to,  we  get: 

*,W  =*,(«.) +  ^At+i*^A(’  +  ...  . 

at  2!  atz 

By  definition:  —  g^Xp+g^Xp  =  =  0.  Moreover,  from  (5.3.19)  and  (5.3.26): 

d?x 

=  9xXp  +  sjtp  +  g^p  +  g^Xp  =  g_Xp  =  (g^  +  g^JXp  =  o . 

Similarly,  all  the  higher  derivatives  of  xp  are  zero  under  the  given  velocity  distribution. 
Thus  the  ICR  is  a  constant  point. 


We  attempt  to  specify  the  global  motion  of  the  NVGT  assembly  induced  by  the 
shape  controls,  as  characterized  by  the  position  and  orientation  71  of  platform  1.  We 
saw  that  instantaneously  this  motion  is  a  translation  whenever  7J’2  =  0  or  a  rotation 
whenever  7X’  ^  0.  We  want  to  find  out  if,  after  a  period  T  of  the  shape  controls,  there 

is  a  net  motion  A71  =f  71  (T)  —  71(0)  of  the  NVGT  assembly.  This  is  equivalent  to  the 
idea  of  using  geometric  phases  (Krishnaprasad  [1990];  Marsden,  Montgomery  &  Ratiu 
[1990]). 

Proposition  5.3.4  (Geometric  Phase  for  Piecewise  Constant  Shape  Controls) 

Consider  the  2-module  NVGT  actuation  scheme  where  the  shape  of  the  (1,2)- 
module  is  kept  constant  (£  =  0)  and  the  shape  of  the  (2,  3)-module  traces  the  rectan¬ 

gular  path  of  fig.  5. 3. 6. a  in  (2:2,3,  </>2,3)~space. 

In  the  case  of  “straight-line”  motion  (711,2  =  0),  if  the  shape-space  path  is  traced 
clockwise,  after  a  period  of  the  shape  controls,  the  assembly  translates  forward  by 


A731en  =  (423}  -4H) 


tan  <^3} 


tan  43 


where  and  $2*3  ,  <$3  are  the  extreme  values  of  2:2,3  and  (j) 2,3  (we  assume  here 

that  x^J  <  xi,2} )  and  ^2]'}  <  •  If  the  shape-space  path  is  traced  counterclockwise, 

the  assembly  translates  backwards  by  the  same  amount. 
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In  the  case  of  “turning”  motion  (7*’ 2  ^  0),  after  a  period  of  the  shape  controls,  the 
assembly  rotates  by 


A2-B2 

a2-b2\ 


where  A2,B2  are  defined  in  the  proof  below. 


Wi(T)  =  - 


COS  <t>. 


{1} 

2,3 


sin</)^3} 


In 


A\  +  Bi  j 


A ! 


COS  (/>. 


{2} 

2,3 


sin4J} 


In 


(a)  (b) 

Fig.  5.3.6:  Piecewise  Constant  Controls  and  Geometric  Phase  for  Translating  2-module  NVGT 


Proof 

Consider  the  following  piecewise  constant  periodic  shape  variations  for  the  second 
module  (let  T  be  the  corresponding  period) : 


<^2,3  (<)  =  < 


r  0, 
a 
0, 

l-A 


for  t  G  [0,  £]; 
for 

foriGt^fj; 

fort€[f  ,Tj; 


^2, 3^) 


a,  for  t  G  [0,  7]; 

0,  for  1  €  [7,  j]; 
-a,  fart€[|,f]; 

0,  forf€[f,T]; 


This  makes  the  shape  trace  (counterclockwise)  a  rectangular  path  in  (^2,3,  </>2,3)-space. 
Let  4,1] ,  ^27  and  027  J027  be  the  extreme  values  of  x2,3  and  0 2,3.  Then  a  =  — 

x2j)  anfl  P  =  7  (027  —  <^27  )■  Those  shape  variations  correspond  to  the  following  shape 
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controls: 


-  h* , 


^’3(t)  =  X2,3  COS  02,3 


(  a  cos  4V,  for  t  E  [0,  ^]; 
Jo,  for  t  €  [f ,  f  ]; 


£2’3(t)  =  -X2,3  Sin  02,3 


,{2} 

-a  COS  </>2,3  i 

I, 

forte[f,f]; 
forte  [f,T]; 

•  J.U1 
-asmcplj, 

for  f  G  [0,  J]; 

o, 

for  t  e  [7,f]; 

a  sin  </>2^, 

for  t  e 

o, 

for  t  E  [^,T]; 

The  corresponding  Wei-Norman  parameters  for  the  (2, 3)-module  are  given  by  (5.3.17) 
as  follows: 


t 

7i’3W  =  7i’3(0)  +  J  ei’3(r)dr  = 


(  7l2’3(0)  =  (j)Q ,  for  t  6  [0,  \ ]; 
7l2-3(0) +/?(*-?),  forte 
2’3(0)  +/3f  =  </43,  forte[f,^]; 
72’3(0)  +  (3(T  —  t),  for  t€  [f  ,T]; 


71 

2 

7i’ 

'V2’ 

7i 


Let  A72,3(t)  =f  J  ^2,3(r)dr.  Then  from  (5.3.17)  we  get: 
o  1 

7s2’3 W  =  -722’3(0)sin  (A72’3(t))  +732’3(0)cos  (A72’3(f)) 


Z 

!‘ 


-  I  £3’3(r)sin  (A72,3(t)  —  A72,3(t))c?t 


Z 

+  J  ^2’3(t)  cos  (A7i’3W  “  A7i’30r))eb- 


=  < 


72’3(°)  -asin^JJJi, 

for  t  e  [0,  f  ]; 

-72,3(0)  sin  (f3(t  -  f))  +  72,3(0)  cos  (/?(t  -  \ ))  -  a  sin  <$3  T> 

for  *6  [?,£]; 

-72,3(0)  sin(/3f)  +72,3(0)cos(/?^)  -  asin^^f  +  asin^^t  -  f), 


-7|’3(0)sin(^(T  -  t))  +7f’3(0)cos(^(T  -  t))  +  a(  sin  <^3  -  sin^)|, 

3 T 
4 


aWm: 


forte[f,T]; 
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In  the  case  that  the  system  translates  (  i.e.  7J’2  =  0),  we  get  from  (5.3.22)  and  (5.3.23): 


a7 lit)  =  7 l{t) 


1  \  / 


,  {1} 

n.cos^if 

~OL  ■  M  b 
sin  </>2  / 

,{1} 

„  cos  <t>ij  T 

a~.  ~nr  4  5 

sm  02,3 

c°s^)>T  cos^jy 

sin  <^>2  3  4  2 

<  ,{2}  ,{1}\’ 

COS  <j>l  j  COS  ^2  3  \  T 

Z1!  )  4> 


E), 


/  COS  0«  o  COS  0i  / 

Vsin^3}  _  «n^j3> 


for  f  G  [0,  j]; 
for  t  G  [z 
for  f  G  [2,  -4 
fortG[f,T]; 


-  ^1- 

4  ’  2  J  ’ 

T  3Tj. 


Then,  after  a  period  of  the  shape  controls,  the  assembly  has  translated  (backwards,  if 
the  relationship  between  the  extreme  values  of  x2,3  and  c/j2 ,3  is  as  shown  in  fig.  5.3.6.b) 

by 


A731(T)  =  « 


/cos4£ 

Vsm<^3} 


tan  4V 


If  we  trace  the  shape-space  loop  in  reverse  (clockwise),  we  get  the  following  global 
motion: 


=  < 


0, 


-a 


,{2} 
COS  <t>2  3 


w 

2,3 


—a 


—a 


sin  <j>, 
cos^^3} 
sin 

m 

2,3 


COS  C 


sin 


(*-?), 


T 
4  ’ 


"2,3 


+  a- 


sin  <j>. 


{i> 

m 


for  t  G  [0,  \ ]; 
fortG 

fortG[|,f]; 

T1),  for  tG  [^,T]; 


Then,  after  a  period  of  the  shape  controls,  the  assembly  has  translated  by 


cos 


(,{2} 

'2,3 


sin  43 


cos4,3}\  T  _  _ 


sm4*s}  '  4 


fr{2}  _  {1}X 

1^2,3  x2,3  1 


tan  4^ 


tan  4,3 


which  is  the  opposite  from  before.  Thus,  if  we  trace  the  shape-space  loop  in  the  re¬ 
verse  direction  (clockwise),  the  system  translates  in  the  opposite  direction  from  before 
(forward) . 
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Consider  now  the  case  g1^2  /  0,  when  the  2-module  NVGT  assembly  will  rotate  around 
the  intersection  of  the  axes  of  the  two  platforms  of  the  (1, 2)-module. 


t  t 

=  7 }(t)  -  7i  (0)  =  J  Cj(r)dT  =  -  sin711’2(0)  J 


e23(r) 

det(A2(T))^T 


COS^'3}  !„ 

— —TIT  m 

Sin  (f> 2  3 


-S2^1n 

sin  </>j7 

-^ln 

sm  <t>lj 


A\  +B\  t 


Ai+Bi  y 
Ai 

Ai+Bi  y 
Ai 

Ai+Bi  y 

A1 


cos423  , 

ln 


sin 


M 

■vU 


2 -#2  * 
i--.  7Tr 


,{  2} 

COS  <^2,3  , 

.  ,{2)  111 
sm  <t>\’ 


A2' 

A2  —  B2  y 


A2—B2^f- 
A2—B2  Y 


for 


1  t  [U, 


for  teff.f]; 


where  Ai  =  yi,2(0) 

2/i, 2(0)  sin  (7i’3(0)  +  Pj)  -  sin711,2(0)  [ 
asin^^j]  and  B2  =  asin7i’2(0)  sin^^-  Then, 
the  assembly  has  rotated  by 


for<e[f,T]; 

(5.3.27) 


1  sin 7i’3(0)  -  sin711’2(0)72’3(0),  Bx  =  asin711,2(0) sin^,  A2  = 
Pi)  -sin711,2(0)[-72,3(0)  sm(p^)jf3  {0)  cos(p^)  -  asin^f  - 

=  asin73’2(0)  sin02^-  Then,  after  a  period  of  the  shape  controls, 
ated  by 


Aj!(t)  =  - 


cos  <p{2} 

sin  ^27 


ln 


Ai  +  Bij 


cos  02^3  A2-B22f 

sin  ^2^  A2  —  B2j 


Proposition  5.3.5  (Geometric  Phase  for  Sinusoidal  Shape  Controls) 

Consider  the  2-module  NVGT  actuation  scheme  where  the  shape  of  the  (1,2)- 
module  is  kept  constant  (£  =  0)  and  the  shape  of  the  (2, 3)-module  traces  an  elliptical 

path  in  (x2,3,  </>2,3)-space,  given  by  the  following  sinusoidal  controls: 

<l>2,3(t)  =  oiwcoswt ,  i2>3  =  cx2u) sinwf ,  y2,3  =  0  .  (5.3.28) 

In  the  case  of  “straight-line”  motion  (7J’2  =  0),  after  a  period  T  of  the  shape 
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controls,  the  assembly  translates  forward  by 


A7; 


t 

l(T)  =  —a2  J 


u  sincjr 


tan(a3  +  07  sinurr) 


dr  , 


where  u  =  (c.f.  fig.  5.3.7).  If  the  shape-space  path  is  traced  in  reverse,  the  assembly 

translates  backwards  by  the  same  amount. 

In  the  case  of  “turning”  motion  (7J’2  /  0),  after  a  period  T  of  the  shape  controls, 
the  assembly  rotates  by 


t 

A7iCn  =  — sin711,2(0)  J 

0 


_ 2 _ d 

det(A2{r)) 


(c.f.  fig.  5.3.8).  If  the  shape-space  path  is  traced  in  reverse,  the  assembly  rotates 
backwards  by  the  same  amount. 

Proof 

Consider  the  following  sinusoidal  periodic  shape  controls  £  3  that  correspond  to  those 
of  equation  (5.3.28): 

£2,3(t)  =  aiwcoswt , 

l )  =  a2u  smut  cos  yl’3  ,  (5.3.29) 

£2,3(t)  =  —  a2us'mut sin72,3  . 

From  (5.3.17): 

7i’3M  =  7i’3(0)  +  «i  sin  ut , 

72 ’3W  =  72,3(0)  cos(«i  sin  ut)  +  72,3(0)  sin(cci  sin  ut)  +  a2(l  -  cos  ut)  COS72’3  , 

732’3W  =  _72,3(0)  sin(ai  sin  ut)  +  72,3(0)  cos(ai  sina>t)  —  a2(l  —  cos  ut)  sin72’3  . 

(5.3.30) 

From  (4.3.21)  and  (5.3.30)  we  see  that  those  shape  controls  correspond  to  a  closed  ellip¬ 
tical  path  in  (^2,3,  </>2,3)-space. 

Let  jI’2  =  7^’2(0)  =  0  so  that  the  2-module  NVGT  assembly  translates. 
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From  (5.3.22)  and  (5.3.23): 


Aj}(t)  =  7 }(t)  -  7i(0)  =  0  , 
^72  W  =  72  (t)  -72(0)  =  °> 


&7l(t)  =  73  W  -  7s (0) 


u >  sin  lot 

tan(a3  +  £*i  sinwr) 


dr  , 


(5.3.31) 


where  0:3  7i’3(0). 


Gamma''  1_3  by  numerical  integration 


Geometric  Phase:  Translation 


1 


(a)  (b) 

Fig.  5.3.7:  Geometric  Phase  for  Translating  2-module  NVGT  (Sinusoidal  Controls) 


From  (4.3.21)  and  (5.3.31)  we  have: 


From  this  we  get: 


<£1  =  7i(0)  > 

*1  =  72(0)  cos  7i1(0)  -  73  sin7i(0)  , 
Vi  =  72(0)  sin  7i  (0)  +  7s1  cos  7^(0)  . 

„  =  7a1  (0)  _  1  r 

1  sin  7i(0)  tan  7^(0)  1  ’ 


(5.3.32) 


(5.3.33) 
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Thus  the  locus  of  the  point  0\  is  the  straight  line  given  by  equation  (5.3.33),  which  is 
perpendicular  to  the  axis  of  platform  1.  Using  Mathematica,  we  can  integrate  (5.3.31) 
numerically  and  verify  that  after  a  period  of  the  shape  controls,  the  2-module  NVGT 
assembly  has  moved  forward  by  a  distance  specified  by  ^73(77)  =  73(77)  —73(0)  (fig. 
5.3.7).  If  we  trace  the  closed  shape-space  path  of  (5.3.29)  in  the  reverse  direction,  the 
assembly  will  move  backwards  by  the  same  distance. 

Let  7*’2  =  7^’2(0)  ^  0,  so  that  the  2-module  NVGT  assembly  rotates  instantaneously 
around  the  point  0^2-  The  position  of  the  assembly  with  respect  to  this  point  can  be 
characterized  by  the  angle  7*. 

Prom  (5.2.24)  and  (5.3.30): 

7i’3(t)  =  7i’2  +  7i’3  =  7i’2(0)  +  7i’3(0)  +  sino ,t  , 

73’3(i)  =  7a’3  -  721,2  sin. 7i ’3  +  7a’2  cos7l2’3 

—  _72,3(0)  sin(o:x  sinnif)  +  72,3(0)  cos(ax  smut) 

—  72,2(0)  +  a2(l  —  cos ut)  sin  ^72,3(0)  +  ax  sintufj 

+  73,2(0)  cos  ^7x’3(0)  4-  ax  sinwfj  . 

(5.3.34) 

Then,  from  (5.3.18): 

det(A2(t))  =  -73,3(t)sin711,2(0)  +  73>2(0)  sin7^’3(t)  .  (5.3.35) 


From(5.3.19): 


and  from  (5.3.21): 


fJW  =  -sin711’2(0) 


e23(t) 

det(A2(t)) 


(5.3.36) 


Z  Z 

a7i  (t)  =  7i  (*)  ~  7i(0)  =  J  £j( r)dr  =  -  sin711,2(0)  J 


det(A2(r )) 


(5.3.37) 


Using  Mathematica,  we  can  integrate  (5.3.37)  numerically  and  verify  that  for  e.g.  7J’2  = 
—  \ ,  after  a  period  of  the  shape  controls,  the  2-module  NVGT  assembly  rotates  clockwise 
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around  the  point  0lj2  by  an  angle  specified  by  =  j}(^)  —  7i(0)  (fig.  5.3.8).  If 

we  trace  the  closed  shape-space  path  of  (26)  in  the  reverse  direction,  the  assembly  will 
rotate  counter-clockwise  by  the  same  angle. 


Gamma/'1_1  by  numerical  integration 


Fig.  5.3.8:  Geometric  Phase  for  Rotating  2-module  NVGT  (Sinusoidal  Controls) 

The  kinematics  of  the  2-module  NVGT  assembly  were  simulated  on  Silicon  Graphics 
IRIS  4D/120  and  Indigo  2  graphics  workstations.  The  primitive  “straight-line”  and 
“turning”  motions  described  above  are  shown  in  fig.  5.3.10.  They  describe  only  the 
average  motion  of  the  system,  however,  they  can  be  very  useful  in  motion  planning, 
since  these  primitive  motions  can  be  synthesized  to  display  more  complex  behaviors  of 
the  system,  like  obstacle  avoidance  (fig.  5.3.11). 

Standard  motion  planning  strategies  (Voronoi  diagrams,  cell  decomposition,  poten¬ 
tial  fields)  can  be  used  for  this  task  (Latombe  [1991]),  but  they  will  result  in  paths  that 
may  require  sharp  changes  of  direction  for  our  system.  Those  paths  could  be  made 
smoother,  as  was  done  in  fig.  5.3.11,  however  this  is  not  necessary.  Notice  that  the 
2-module  NVGT  can  execute  very  sharp  turns,  even  rotate  (on  the  average)  in  place. 
Thus  no  constraints  on  the  curvature  of  the  average  path  of  the  system  need  to  be  placed. 
Moreover,  by  properly  adjusting  the  size  of  the  shape-space  loop,  the  “stride  length” ,  as 
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it  is  quantified  by  the  geometric  phase,  can  be  varied.  If  greater  accuracy  of  motion  is 
needed,  then  smaller  “steps”  can  be  taken  (c.f.  related  results  in  (Leonard  [1994])). 

Suppose  then  that  we  want  to  move  from  <?i  =  gi(xi,yi,</> i)  G  SE( 2)  to  g\  = 
gi(xii  j/i,  (pi)  G  SE(2),  corresponding  to  points  O i  and  O i  respectively.  This  can  be  done 
by  synthesizing  the  primitive  “straight-line”  and  “turning”  motions  described  above  in 
three  steps,  as  follows: 

i)  First,  “turn”  in  place  (i.e.  move  with  7J’2  =  |j  from  the  initial  orientation  (pi  to  an 
orientation  along  the  line  O\0i  (fig.  5.3.9). 

ii)  Then,  “move  straight”  (i.e.  with  7J’2  =  0)  along  the  line  O1O1  from  point  0\  to  the 
desired  point  0\ . 

iii)  Finally,  “turn”  in  place  until  the  final  orientation  (pi  is  reached. 

Since  controllability  has  been  established  for  SE( 2),  we  know  that  any  such  motions 
are  possible,  as  long  as  we  stay  away  from  nonholonomic  singularities.  Thus,  the  shape 
variations  used  to  implement  the  above  primitive  motions,  should  be  designed  so  that 
this  happens. 


y 


o 


X 


Fig.  5.3.9:  Motion  Planning  for  2-module  NVGT  moving  on  the  plane 
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(a)  “Straight-line”  (b)  “Turning” 

Fig.  5.3.10:  Primitive  motions  of  the  2-module  NVGT 


Fig.  5.3.11:  Obstacle  Avoidance  with  the  2-module  NVGT 
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5.4  SE(2)— Spiders,  SE(2)— Rings  and  other  SE(2)-Snake  Kinematic  Topolo¬ 
gies 

The  framework  presented  in  section  4.2  is  not  restricted  to  nodes  in  serial  (chain) 
arrangement.  It  can  be  applied  to  non-serial  tree-like  and  ring-like  arrangements  of 
nodes. 

In  section  5.4.1  we  analyze  the  case  of  a  tree-structured  assembly  in  SE( 2).  We 
refer  to  those  assemblies  as  SE(2)-Spiders.  In  section  5.4.2  we  analyze  the  case  of  a 
ring-structured  assembly  in  SE( 2).  We  refer  to  those  assemblies  as  SE(2)-Rings.  After 
developing  their  kinematics,  we  show  how  motion  control  for  such  assemblies  can  be  re¬ 
duced  to  the  corresponding  problem  for  the  SA(2)-Snake  (NVGT)  that  was  discussed  in 
section  5.3.2.  In  section  5.4.3  we  consider  the  case  of  a  1-module  SE(2)-Snake  assembly, 
where  the  Codimension  1  Constraint  Hypothesis  introduced  in  section  4.2.2  is  relaxed  so 
that  one  of  the  nodes  is  constrained  by  one  nonholonomic  constraint  as  usual,  but  the 
other  one  is  constrained  by  two  such  constraints.  This  provides  us  enough  constraints  to 
allow  the  expression  of  the  global  velocity  of  the  assembly  as  a  function  of  the  shape  and 
the  shape  variations,  which  was  not  possible  previously,  as  discussed  in  section  5.2.2. 


5.4.1  The  3-module  SE(2)-Spider 

Consider  the  4-node  assembly  of  fig.  5.4.1  consisting  of  one  central  node  connected 
by  linear  actuators  in  a  planar  parallel  manipulator  configuration  with  each  of  the  3 
other  nodes.  These  nodes  are  subject  to  one  nonholonomic  constraint  per  node,  due  to 
idler  wheels. 

The  trajectory  of  each  node  is  a  curve  j,cG  =  SE(  2),  while  its  velocity  is  a  curve 

i 

£  C  Q  =  se(2),  such  that: 

i 


9.  =  TeLg.  -f.  =  gX.  ,  *  =  !,••■  ,4. 


(5.4.1) 


i  i  it 

A  pair  of  nodes  i  and  j  of  the  chain  constitutes  the  ( i ,  j)-th  module  and  its  shape  g  C  G 


*,3 


and  shape  variation  £  C  Q  are  defined  by: 


and 


9.  .=9.  9.  , 

*.J  l  3 


9.  .  =TeLg  •£.  .  =  g.  X.  .  . 

«,J  i ,}  1,3  1,3  1,3 


(5.4.2) 

(5.4.3) 
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Fig.  5.4.1: 

From  this,  we  can  easily  see  that 

and 

£.  =  £.  +  Adg- it,  for  *  =  2, 3, 4  . 

1  M  l,i  1 

The  system  is  subject  to  the  following  3  nonholonomic  constraints: 

4(y  =  o , 

A(  y  =  0 , 

=  o. 


The  3-module  S'JF(2)-Spider 


o=oo 

*  yiyi  ,j 


(5.4.4) 


(5.4.5) 


(5.4.6) 


Consider  the  composite  velocity  vector  of  the  system  (  =  (£lT  £1,2T  £li3T  ^1i4T  )T  and 

let  the  global  velocity  of  the  assembly  be  characterized  by  £  ,  while  the  shape  variations 

£  ,  £  ,  £  of  the  3  modules  are  the  system’s  controls. 

n,2’sl,3,'T,4  J 

The  constraints  (5.4.6)  can  be  put  in  matrix  form  as: 


(5.4.7) 
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where  A  =  ( ,  A\)  with 


010000000 
^i=[0000  1  0000 

000000010 


and 


X 

^3 

cos  7,  ’ 
1,3 

sin  7^  ’ 

_7i4 

cos  7, 

1,4 

sin  7, 

-73 

cos  7X 

sin71’ 

Then,  the  composite  velocity  vector  can  be  partitioned  as  £  =  ( 9 ) ,  with  = 

(£i,2T  £i,3T  ^i,4t  )T  an(j  Then,  when  A 2  is  nonsingular,  the  constraints  (5.4.6) 

take  the  form: 


(5.4.8) 


C2  = 

£  =  =  -A,-1 

£l53 

1  2 

It  is  easy  to  see  that  A2  is  singular  whenever  the  axes  of  platforms  2, 3  and  4  intersect  at 
one  point  or  are  parallel.  Motion  control  for  this  system  will  use  periodic  ^’2,^1,3  and 

in  generating  appropriate  motions  of  the  system. 

However,  a  simpler  strategy  would  be  to  reduce  the  motion  control  for  the  3-module 

SE(2)-spider,  to  that  of  motion  control  for  the  2-module  NVGT  that  was  examined  in 

detail  in  the  previous  section.  To  do  this,  we  will  characterize  the  global  motion  of  the 

system  by  not  while  regarding  ^  ^  and  ^  ^  as  the  shape  controls.  The  real  shape 

controls  £1,2,£1,3  will  be  used  to  implement  the  desired  variation  of  £  and  £ 

2  2  2,3  3 ,4 

From  the  system’s  kinematics  we  have 


9 

9 


3 

4 


9 

9 

9 


2.3 

3.4 

2.4 


=99  =99  9 

3  3,4  J2^2,3  3,4 

—  g-'g  o-1o  =  g-1 

* 2  *1*1  ^3  1,2 

—  9 ~X9  9_19  =  91 

y3  *1*1  y4  yi,3 

~99 

z,3a3,4 


9 

9 


1.3  ’ 

1.4  ’ 


(5.4.9) 
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Then: 

[«4i  5  A2]  =  A3  ,  [Ai  ,  ^3]  =  0  ,  [A2  ,  ^3]  =  0  .  (4.3.29) 

Proposition  4.3.4 

The  algebra  Q  =  h(3)  is  nilpotent  (thus  solvable)  and,  from  Proposition  2.3.14,  any 
g  £  G  —  3)  has  a  global  Wei-Norman  representation  of  the  form  (4.3.3)  with 


7i\  /  1  0  0\ 

72  =  0  10 
73  /  v  “72  0  1  / 

Equation  (4.3.30)  is  solvable  by  quadratures: 


(4.3.30) 


72  (t)  =72(0)  +  J  £2(r)dr  , 

0 

t  t 

73 (t)  =  73(0)  -  J  72 (T)^(r)dr  +  J  ^( r)dr 

0  0 

t  t  T  t 

=  73(0) -72(0)  J  ^{(j)da-  J  ^1(T)(^J  t2(a)dc^)  dr  +  [  Z3iT)dT  ■ 

0  00  0 


(4.3.31) 


Proof 

Since  Q  =  h( 3)  is  nilpotent,  the  existence  of  a  global  representation  is  immediate  by 
(Wei  &  Norman  [1964]).  To  see  that  it  has  the  form  (4.3.3)  with  coefficients  given  by 
(4.3.30)  and  (4.3.31),  we  compute  the  RHS  of  (4.3.5),  using  (4.3.29)  and  (2.3.20): 

eadl-'n^)A1=A1+j2A3, 

ead(-y3As)ead(-y2A2)Ai  =  ^  +  ?  (4.3.32) 

ead(-y3A3)A2  =  A2  . 

From  (4.3.1),  (4.3.2),  (4.3.5)  and  (4.3.32),  we  have: 

£  —  Cl-^l  +  £2-4-2  +  ^3-^3 
=  7l(^l  +  72-43)  +  72^2  +  73^3  • 
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The  corresponding  shape  variations  are 


£  =  £  +  Ada-i  £  , 

S3  S2,3  fl3,4  3  ’ 

L  =  L  „  +  Adg-1  £  =  £  +  Ad„- 1  £  +  Ad„-i  £  , 


’3,4 


3,4 


3,4  a3,4  ’2,3 

^2,3  ~  ^1,3 

^3,4  _  ^1,4  ^®i,3^1,3  ' 


2,4 


(5.4.10) 

(5.4.11) 

(5.4.12) 

(5.4.13) 


The  nonholonomic  constraints  (5.4.6)  can  be  expressed,  using  (5.4.10)  and  (5.4.11)  as 


A<W)C  =  °’ 


(5.4.14) 


where  the  composite  velocity  vector  is  now  £  =  (£2T  £2,3T  £3,4T  )T  and  =  (^2 ,  _4:) 


with 


0  0  0  0  0  0’ 

Ai  =  [  0  1  0  0  0  0 

— 7g’4  COS73’4  sin7j’4  0  1  0 


and 


0 


1 


0 


A2=(~7^  cos7^  smru 

-73  cos  7:  sm71 


Then,  the  composite  velocity  vector  can  be  partitioned  as  £  =  ( ^ ) ,  with  £i  = 
(£2,3T  £3,4T  )t  and  £2  =  £2.  Then,  when  A2  is  nonsingular,  the  constraints  (5.4.14) 


take  the  form: 


£2  —  -A2  4Ai£i  . 


(5.4.15) 


The  motion  control  strategies  that  were  devised  for  the  2-module  NVGT  can  be  applied 
to  this  system  in  terms  of  periodic  controls  for  £2  ^  and  £g  ^ .  They  can  be  implemented 

by  solving  (5.4.12)  and  (5.4.13)  for  £  ,£  and  £  .  This  can  be  done  by  e.g.  setting 

1 ,2  1)3  1  )4 

£  =0,  when 

1,3 

£  =  -Adg  £  , 

1,2  S2,3  2,3 


^1,4  ^3,4  ' 
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o 

.? 


Fig.  5.4.2:  The  3-module  S£(2)-Ring 
5.4.2  The  3-module  SE(2)-Ring 

Consider  the  3-node  ring  assembly  of  fig.  5.4.2.  Each  node  is  connected  with 
both  of  the  others  in  a  planar  parallel  manipulator  configuration  and  is  subject  to  a 
nonholonomic  constraint  coming  from  idler  wheels  rolling-without-slipping  on  the  plane 
supporting  the  assembly. 

From  the  system  kinematics  we  get: 


^2  ^1^1,2  ’ 


^3  ^2^2,3 


From  the  loop  equation  we  have: 


g  = g  9 

3 1,3  al,2^2,3 


The  corresponding  velocities  are 


^2  ^12^  ’ 
2  1i2  1 


£  =  £  +  Adg~i  c  =£  +  Ada-i  t  +  Ad(„  „  \-i£ 

S3  ’2,3  ^2,3^2  ’2,3  \  341,2  (S1,292,3}  S1 


(5.4.16) 


(5.4.17) 


(5.4.18) 
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Furthermore,  the  shape  variations  should  satisfy  the  holonomic  constraint  coming  from 
the  loop  equations: 


+  Adg-l 

y2,3 


(5.4.19) 


The  nonholonomic  constraints  of  rolling-without-slipping  can  be  expressed  as: 


-4(0  =  0,  for*  =  1,2,3. 

i 


(5.4.20) 


The  nonholonomic  constraints  (5.4.20)  can  be  expressed,  using  (5.4.18)  as 


A(91>2,92t3)(  =  0,  (5.4.21) 

where  the  composite  velocity  vector  is  now  £  =  (£lT  £1,2T  £2,3T  )T  and  A  =  (A2,  A\) 
with 

/  0  0  0  0  0  0\ 

;4i  =  0  1  0  0  0  0 

y— 7g’3  COS72’3  sin7^’3  0  1  0/ 

and 

/  0  1  0  \ 

A2  =  -7^  cos7P  sin7P  • 

\-73  cos  7i  3  sinTi  / 

Then,  the  composite  velocity  vector  can  be  partitioned  as  £  —  ( ) ,  with  £1  = 

( £1,^t  £2,3T  )t  and  £2  =  £L.  Then,  when  A2  is  nonsingular,  the  constraints  (5.4.21) 
take  the  form: 

£2  =  -A2  %£i  .  (5.4.22) 

It  is  easy  to  see  that  A2  is  singular  whenever  the  axes  of  all  3  platforms  are  parallel  or 

intersect  at  one  point.  Motion  control  strategies  for  the  2-module  NVGT  in  terms  of 

periodic  controls  for  £  and  £  can  be  applied  to  this  system.  However,  now  £  will 
1 ,2  2,3  1 ,3 

have  to  be  adjusted  using  (5.4.19)  to  account  for  the  loop  constraint. 


5.4.3  The  1-module  SE(2)-Snake  with  more  than  one  constraint  per  node 

In  section  4.2.2  we  introduced  the  Codimension  1  Constraint  Hypothesis,  according 
to  which,  every  node  of  a  G-Snake  was  constrained  to  evolve  on  an  (n  —  l)-dimensional 
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subspace  h  of  the  Lie  algebra  Q,  which,  in  addition,  was  not  a  subalgebra  of  Q.  The 
constraints  £  €  h  could  then  be  expressed  as  _4^(£)  =  0,  for  some  Abk  €  Q*,  such  that 
h  =  Ker(4). 

In  this  section  we  examine  how  this  hypothesis  can  be  relaxed,  by  allowing  some 
nodes  of  the  assembly  to  evolve  on  a  subspace  h  of  Q  of  dimension  n  —  r,  for  r  >  1. 
Suppose  h  =  Ker(sp{^i , . . . ,  Abkr}) .  Then  the  requirement  that  £  €  h  can  be  expressed 
as  a  set  of  constraints 

.Abfcl(£)=0,  ...  ,4r(£)  =  o, 

for  a  set  of  r  linearly  independent  elements  of  Q*.  However,  in  this  case,  the  system  may 
have  to  evolve,  not  on  the  whole  of  G,  but  on  a  subspace  of  G. 

As  an  example  of  this  case,  we  consider  a  1-module  NVGT  assembly  where  two 
platforms  with  idler  wheels  are  connected  with  3  legs  in  a  planar  parallel  manipulator 
configuration  (fig.  5.1.1).  One  platform  (say  platform  2)  has  two  wheels  that  move 
independently  of  each  other,  so  they  provide  a  nonholonomic  constraint  from  rolling- 
without-slipping  exactly  similar  to  the  previous  assemblies  of  this  chapter.  The  other 
platform  (platform  1)  has  two  wheels  which  are  joined  by  a  shaft,  which  forces  them  to 
rotate  simultaneously.  Thus  they  are  not  independent  of  each  other  any  more,  but  they 
are  otherwise  free  to  rotate.  Such  an  arrangement  prevents  both  sideways  motion  and 
rotation  of  the  platform,  which  is  only  able  to  translate  along  the  perpendicular  to  the 
axis  of  the  wheels. 

The  system  kinematics  are  now 


g2  =  9x912  (5.4.23) 

and 

£,  =  (,  ,  +  Adg-i  £  •  (5.4.24) 

2  1,2  1>2  1 

The  two  nonholonomic  constraints  imposed  by  the  motion  of  the  wheels  of  platform  1 
can  be  expressed  as 

4(e1)  =  o, 

(5.4.25) 

•4(g  =  0  , 
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while  the  constraint  imposed  by  platform  2  can  be  expressed  as 


=  0  • 


(5.4.26) 


Consider  the  composite  velocity  vector  of  the  system  (  =  (£lT£1,2T)T  and  let  the  global 
velocity  of  the  assembly  be  characterized  by  ^ ,  while  the  shape  variations  £  of  the 
module  are  the  system’s  controls. 

The  constraints  (5.4.25)  and  (5.4.26)  can  be  put  in  matrix  form  as: 


^12)C  =  0, 


(5.4.27) 


where  A  =  ( A2  ,  A\)  with 


and 


A2  = 


'0  0  O' 
Ax  =  1  0  0  0 
0  1  0 


0 

1.2 

.  ^3 


0 

1 


cos 


1,2 


0 
0 

sin7^’2 


Then,  the  composite  velocity  vector  can  be  partitioned  as  £  =  (£*),  with  £1  =  f1,2 

and  ( 2  =  The  matrix  A2  is  singular  whenever  detA2  =  sin 7 \’2  =  0.  When  A2  is 
nonsingular,  the  constraints  (5.4.27)  take  the  form: 


((l) 

fo\ 

e2 

=  -A2-%Ci  =  -r^T2 

0  . 

(5.4.28) 

kj 

sin  7X 

Vi ) 

Only  £ 1,2  is  nonzero,  as  expected  by  the  constraints,  thus  the  assembly  moves  perpendic¬ 
ular  to  the  axis  of  the  first  platform.  Configurations  outside  this  line  cannot  be  reached. 

Assume  that  the  shape  of  the  1-module  NVGT  assembly  traces  a  rectangular  path 
in  shape-space  similar  to  the  one  described  in  Proposition  5.3.4,  i.e. 


'  0,  for  t  e  [0, 

1,2  I  0,  for  *€  [£,£]; 

1  I  0,  for  t  e  [f ,  ]; 

>~P,  for£G[2T,T]; 


a  cos 

0, 


f»{1} 
a, 2  5 


L  0, 


-a  cos  </>f22  , 


for  t  €  [0,  j ]; 

for  *6[E  |]; 
for  te[f,¥]; 
for  t  £  [f  ,T]; 
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This  makes  the  shape  trace  (counterclockwise)  a  rectangular  path  in  (2:1,2,  <^i,2)-space. 
Let  x\*2,xi^2  and  »$i?2  be  the  extreme  values  of  2:1,2  and  <f> x,2.  Then  a  —  — 

*1*2 )  and  P  =  H<t> la  ~ 

The  corresponding  Wei-Norman  parameters  for  the  (l,2)-module  are  given  by  (5.3.17) 
as  follows: 


(  7i1,2(0)  = 


for  t  £  [0, 


1.2/^s  1,2/q,  f  £1,2/  w  _  J  7l’  (0 )  +  (3{t  4), 

7,  W- 7,  (0)  +  /«,  ^-i7u(0)+/35  =  ,g,  fa.eStt]. 


for  t  £  [f  ,%i; 


l  7i’2(0)  +  P{T  ~  t), 


forte  A, T]; 


Then  we  get  from  (5.4.28): 


t  t 

A7 !(<)  =  73  W  -  731(0)  =  J  ^(r)rfr  =  -  J  ■ 

0  0 


72<x> 

sin  7i ’2  (v ) 


•dr 


a 


COS^iV  £ 

,|l}L 

COS  V  T 

cos^^  cos#«  J.  {  ;  [T  31], 

sin*d>  4  +  Bin^>  1  2  b  I°r  I  fc  [  2  j  4  J  > 

/  COS  0  J  22^  COS  <£j  y  \  JT 

^  ^  T ; 


for  t  6  [0,  f]; 

forie[?,|]; 


^sin  <t>lj 


sin  1 


for  te[f,T]; 


Then,  after  a  period  of  the  shape  controls,  the  assembly  has  translated  by 


A7im  _  J cos^&'  _  1  _  ,,m  _ xuh( _ L 

73(  Lin^'fl  Sin^2')4-(X^  X1,2  Vtan^| 


{22}  tan<^2} 


If  we  trace  the  shape-space  loop  in  reverse,  the  assembly  moves  in  the  opposite  direction. 
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CHAPTER  SIX 


DYNAMICS  OF  AN  SE(2)-SNAKE  ASSEMBLY 


6.1  Introduction 


Fig.  6.1.1:  The  Roller  Racer 

The  effect  of  dynamics  on  systems  of  the  G-snake  type  that  we  examined  kinemat¬ 
ically  in  chapter  5,  can  be  seen  by  considering  the  1-module  5J5(2)-snake  system  of  fig. 
6.1.1.  This  system  is  sold  commercially  under  the  name  “Roller  Racer” and  its  design  is 
based  on  a  U.S.  patent  held  by  W.  E.  Hendricks  [Patent  #  3663038  of  May  16,  1972]. 
The  system  consists  of  a  seat,  with  a  pair  of  idler  wheels  mounted  in  its  rear  and  with 
an  elongated  steering  arm  pivoted  to  the  front  of  the  seat,  with  a  pair  of  idler  wheels 
also  mounted  on  it,  on  the  rear  of  the  pivot  axis.  A  rider  provides  the  propulsion  and 
steering  mechanism  of  this  vehicle,  by  swinging,  with  his  arms  and  feet,  the  steering  arm 
around  the  pivot  axis  and  by  leaning  his  body  to  the  left  and  to  the  right,  following  the 
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Fig.  6.1.2:  The  Roller  Racer  Model 


swinging  of  the  arm. 

The  model  of  fig.  6.1.2  will  be  used  in  our  analysis.  Two  planar  platforms  with 
centers  of  mass  (c.o.m.)  located  at  points  0\  and  O2  are  connected  with  a  rotary  joint 
at  0 1,2-  A  pair  of  idler  wheels  is  attached  on  each  of  the  platforms,  with  the  axis  of  the 
wheels  perpendicular  to  the  line  connecting  the  c.o.m.  with  the  joint.  A  coordinate  frame 
centered  at  the  c.o.m.  and  with  its  x-axis  along  the  line  OtOit2  for  i  =  1,2  connecting 
the  c.o.m.  with  the  joint,  will  be  used  to  describe  the  configuration  of  each  platform 
with  respect  to  a  global  coordinate  system  at  some  reference  point  O.  For  simplicity,  it 
will  be  assumed  that  the  axis  of  the  wheels  passes  through  the  c.o.m.  of  each  platform. 

The  effects  of  the  rider’s  body  motion  will  be  ignored  at  first  approximation.  Exper¬ 
iments  with  the  Roller  Racer  show  that,  even  though  those  body  motions  may  amplify 
the  resulting  motion  of  the  system,  the  fundamental  means  of  its  propulsion  is  the  pivot¬ 
ing  of  the  steering  arm  around  the  joint  axis  and  the  nonholonomic  constraints  coming 
from  the  wheels’  rolling-without-slipping  on  the  plane  supporting  the  vehicle.  In  this 
respect,  this  system  is  very  different  from  the  Snakeboard,  a  variation  of  the  skateboard, 
where  the  motion  of  the  rider  is  essential  for  the  propulsion  of  the  system  (Lewis  et  al. 
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[1994]).  Riderless  prototypes  of  the  Roller  Racer  built  at  the  Intelligent  Servosystems 
Laboratory  verified  this.  The  propulsion  and  steering  mechanism  in  these  vehicles  comes 
from  a  rotary  (stepper)  motor  at  the  joint  Oii2,  whose  torque  can  be  considered  as  the 
control  of  our  system.  As  discussed  in  section  5.2,  the  purely  kinematic  analysis  of  such 
a  system  does  not  allow  us  to  determine  the  global  motion  of  the  system  by  just  the 
shape  variations  (the  joint  velocity  in  this  case),  since  (unlike  the  2-module  case)  it  does 
not  possess  a  sufficient  number  of  nonholonomic  constraints  for  this  to  happen  (c.f.  Re¬ 
marks  5.2.6).  Our  goal  here  is  to  complement  this  kinematic  analysis  with  the  dynamics 
of  the  system,  which  will  provide  the  necessary  information.  Thus,  certain  fundamental 
behaviors  of  the  system  (“straight-line”  motion,  “turning”  motion)  can  be  achieved  by 
proper  oscillatory  relative  motions  of  the  two  platforms.  In  both  numerical  simulations 
and  experiments  with  prototypes,  we  observed  such  behaviors. 

An  alternative  to  the  usual  approach  of  solving  the  full  Lagrange-d’  Alembert  equa¬ 
tions  of  motion  of  the  system  is  considered  here.  In  (Bloch,  Krishnaprasad,  Marsden  & 
Murray  [1994]),  the  notion  of  the  momentum  map  is  examined  for  systems  with  non¬ 
holonomic  constraints  and  symmetries  and  its  evolution  law,  the  momentum  equation, 
is  derived  from  the  Lagrange-d’  Alembert  equations.  By  applying  this  method  to  the 
problem  at  hand,  a  useful  decomposition  of  the  equations  of  motion  is  obtained:  Given  a 
shape-space  trajectory  (which  corresponds  to  the  controls  of  our  system),  first  we  com¬ 
pute  the  nonholonomic  momentum  from  the  momentum  equation.  This  only  involves 
the  solution  of  a  linear  ordinary  differential  equation.  Subsequently,  we  use  the  momen¬ 
tum  to  reconstruct  the  group  trajectory,  which  corresponds  to  the  global  motion  of  the 
system.  The  corresponding  velocities  depend  linearly  on  the  momentum.  This  process  is 
very  useful  for  the  derivation  of  motion  control  laws  for  this  system  and  can  be  extended 
to  1-module  5.E(2)-snakes  with  more  general  shape-changing  mechanisms. 


6.2  Kinematics  of  the  Roller  Racer 

(cos Oi  —  sin Oi  Xi  \ 

sinSj  cos  Oi  yi  €  SE( 2),  for  *  =  1,2,  be  the  configuration  of  plat- 

0  0  1  j 

form  i  with  respect  to  the  global  coordinate  frame  at  O,  where  xt,  y%  and  9%  are  indicated 
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(COS 01,2  —  sin 0\t2  xi,2  \ 

sin0i,2  cos  0it2  yi,2  1  S  SE( 2)  be  the  configuration  of 
0  0  1/ 

platform  2  with  respect  to  the  coordinate  frame  of  platform  1  at  0\.  Because  of  the 
special  structure  of  the  joint,  we  have 

®i,2  =  dx  +  d2cos0i,2  ,  2/1,2  =  sin 01,2  ,  (6.2.1) 

where  di  is  the  distance  of  Ot  from  the  joint  Oi,2.  We  consider  non-negative  d]  and  d2. 
In  fact,  we  assume  d\  >  0.  However,  we  allow  for  the  case  d2  =  0  and  we  examine  it  in 
detail. 

Since  the  platforms  form  a  kinematic  chain,  we  have 

Q2  =  9l91  2  ,  (6.2.2) 


thus 


02  =  01  +  01,2  > 

x2  —  X\  +  o:i,2  cos  0i  —  2/1,2  sin 0i  =  xi  +  di  cos0i  +  d2  cos02  ,  (6.2.3) 

2/2  =  2/i  +  Xi, 2  sin 0i  +2/1,2  cos  0i  =  yx  +  dx  sin0i  +  d2  sin02  . 


The  system  kinematics  are  a  special  case  of  those  of  the  VGT  assembly,  i.e.  for  £.  = 


(0  -0, 

di  o 

\  0  0 


€  Q  =  se( 2)  we  have: 


9.  =  9.i.  ,  *  =  1,2 
2  2  2 


and 


0  =  g  £ 

^1,2  al,2sl,2 


.  /o  -i 

where  £  =  0i,2  1  0 

\0  0 


°\ 

d2  ■  Moreover,  we  can  show  that 

0/ 


(6.2.4) 


(6.2.5) 


£2  =  Adg 


1,2 


,t.+t 


1,2 
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(6.2.6) 


where 


Adg-iAl  =  A\  —  73^2  +  72^3  i 
Adg-iA<2  =  cos  71.4.2  -  sin  71^3  , 
Adg-iA3  =  sin 7^2  +  cos  71 M3  , 


(6.2.7) 


with  7 i  the  Wei-Norman  parameters  of  g.  Alternatively, 

^2  =  +  0i,2  1 

x2  =  &i+  ii)2cos0i  -  yi^s'mdi  -  (27,2  sin  0X  +  yi)2cos0i)0i 

=  x\  —  6\\d\  sin +  d2  sin(0i  +  ^1,2)]  ~  ^1,2^2  sin(0i  +  ^1,2)  ,  (6.2.8) 

02  =  Vi  +  £1,2  sin 0!  +  yi,2cos0i  +  (2:1,2  008  0!  -  yi)2sin0i)0i 
=  01  +  01  [dl  COS  01  +  d2  COS (01  +  01,2)]  +  01,2^2  cos (01  +  01,2)  ■ 

From  (6.2.3)  we  see  that  the  configuration  space  for  the  Roller  Racer  system  is  Q  — 
SE( 2)  x  S1.  Consider  the  space  <S  =  S1  x  S1  and  consider  the  projection  7r  :  Q  — >  S.  Let 
Q  be  parametrized  by  j/i,0i),0i>2)  with  0  (xi,yi,0i)  E  G  and  5  be  parametrized 

by  2)-  Then  vq  E  TqQ  can  be  represented  as  {xi,y\JhJha)-  It  can  be  easily  seen 

that  Tn  is  onto,  thus  7r  is  a  submersion.  Thus,  n  :  Q  — >■  S  is  a  bundle  with  vertical  space 

Vq  =  Ker(T97r)  =  {n9  E  TqQ  |  0X  =  0, 0i,2  =  0}  . 

The  constraints  specify  an  Ehresman  connection  (Bloch,  Krishnaprasad,  Marsden 
&  Murray  [1994]),  (Marsden,  Montgomery  &  Ratiu  [1990])  on  this  bundle.  By  also 
considering  the  dynamics  (or  more  precisely  the  momentum  equation) ,  we  can  synthesize 
a  principal  fiber  bundle  connection  for  this  system. 

Consider  the  usual  bases  {Mi, M2, M3}  for  Q  =  se(2)  and  {Mi,M2,M3}  for  its  dual 
space  Q* .  The  nonholonomic  constraints  on  the  wheels  of  the  two  platforms  can  be 
expressed  as: 

£3  =  *43 (Cx)  =  — ®i  sin  0i  4-  2/1  cos  0i  =  0  ,  (6.2.9) 

=  M3(£  )  =  —£2  sin  02  +  1/2  cos  02  =  0  ,  (6.2.10) 

3  A 
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Prom  (6.2.8)  and  (6.2.10),  we  get 


—  ^3(^3)  —  — ^1  sin(0i  +  ^1,2)  +  Vi  cos(6>i  +  0ii2)  +  0i(di  cos0ij2  +  d2 )  +  0i)2d2  =  0  . 

(6.2.11) 

Observe  that  for  d2  =  0,  neither  one  of  the  constraints  (6.2.9)  and  (6.2.11)  involves  012. 
Prom  (6.2.9)  and  (6.2.11),  we  get: 

it  =  *43(£J  =  —  (®i  cos 61+yi  sin#!)  sin^lj2  +  (di  cos  0i,2  +  d2)0i  +  d20i>2  =  0  .  (6.2.12) 

Proposition  6.2.1 

The  nonholonomic  constraints  (6.2.9)  and  (6.2.11)  are  linearly  independent  for  all 

qeQ. 

Proof 

We  can  rewrite  the  constraints  in  the  form: 

/  *1  \ 

f  -sin0i  cos  0i  0  0)  ji 

V  -  sin(0x  +  0\ ,2)  cos(0i+0i2)  dicos0i2  +  d2  d2  )  9\ 

W 

Consider  the  determinants  of  all  possible  2x2  minors: 

Ai  —  —  sin  9i  cos(#i  +  0i>2)  +  cos  9\  sin(0i  +  0i;2)  =  sin  9\ >2  , 

A2  =  —  sin@i(di  cos  9\  2  +  d2)  , 

A3  =  —  sin  9 1  d2  , 

A4  =  cos0i(di  cos0i]2  +  d2)  , 

A5  =  cos  0i  d2  , 

A6  =  0. 

When  d2  7^  0,  consider  Ai,...,A5.  If  sin01;2  7^  0,  then  Ax  7^  0  establishes  linear 
independence.  If  sin0ij2  =  0,  then  A3  ^  0  or  A5  ^  0.  When  d2  =  0,  consider  Ai,  A2 
and  A4.  If  sin0ii2  7^  0,  then  Ai  7^  0.  If  sin0i;2  =  0,  then  cos0ii2  7^  0  and  we  have  either 
A2  7^  0  or  A4  7^  0.  Thus,  the  null  space  of  this  matrix  is  always  of  dimension  2. 
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Define  the  constraint  one-forms: 
cjg  =  —  sin  da: i  +  cosOidyi  , 

tOg  =  -  sin(0i  +  Oi%2)dxi  +  cos(0i  +  0lt2)dyi  +  (di  cos  0i,2  +  d2)d$ x  +  d2d01<2  . 

(6.2.13) 

The  constraint  distribution  Vq  is  the  subspace  of  TqQ  which  is  the  intersection  of  the 
kernels  of  the  constraint  one-forms,  i.e. 


Vq  =  Ker  LOg  fl  Ker  .  (6.2.14) 

Since  the  constraints  are  linearly  independent,  we  know  that  Vq  is  always  2-dimensional. 
Next,  we  will  specify  a  basis  for  it. 

Proposition  6.2.2 

The  constraint  distribution  Vq  C  TqQ  is 

^  =  sp{^,^},  (6-2.15) 


where  in  the  case  d2  ^  0  : 


d  0  d 

(a  =  d2(  cos0i—  +sin6>i— )  +  sin0i,2  ^ —  , 
w  dx  i  dyi  39 1>2 

d  d 

m  ( di  cos6>i,2  +  d2)——  , 

OV\  OU  1,2 


(6.2.16) 


while  in  the  case  d2  =  0  : 


SQ 


d 

<901,2 


(6.2.17) 


Proof 

Let  Xq  =  v\ +v2-^  +V3^  +Vi E  Vq,  with  Vi  £  M,  be  a  vector  field  in  the 
constraint  distribution.  By  definition  it  should  annihilate  both  constraint  one-forms. 
Consider  first  the  case  d2  ^  0.  From  the  constraints  we  have: 

=  —v\  sin0i  +  v2  cos  0i  =  0  , 

u)l{Xq)  =  -(vi  cos 0i  +  v2sin01)sin0ii2  +  {dx  cos0i,2  +  d2)v 3  +  d2vx  =  0  . 
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Let  vi  =  d2  cos  0iv5,  v2  =  d2  sin0iii5  and  114  =  sin0i,2n5  -  dl  cos^-2+d^  Vs ,  Then: 

d  d  c) 

Xq  =  v5  [d2  COS  01- - 1-  d2  sin 91- - 1-  sin0i  2  — — 1 

OX  1  9yi  901,2 

+  ^3  [d2  ~  (di  cos  01>2  +  d2)  7^]  , 

for  arbitrary  113, 115,  satisfies  both  constraints.  The  two  vector  fields  are  obviously  linearly 
independent. 

Consider  the  case  d2  =  0.  The  constraints  now  become: 

Ulq{Xq)  —  —  irsin#!  +  tl2COS0i  =  0  , 

u>q(Xq)  —  —(iii  cos  0i  +  v2  sin0i)  sin0i,2  +  di  cos  0i,2ti3  =  0  . 

Let  Hi  =  di  cos0iii5,  v2  =  di  sin0iii5  and  113  =  sin0i,2ii5.  Then: 

d  d  d  d 

Xq  =  H5[dicos0i— —  +  d:sin0i—  +sin0i,2— 1  , 

dxi  dyi  90i  90i,2 

for  arbitrary  114,115,  satisfies  both  constraints.  The  two  vector  fields  are  obviously  linearly 
independent. 

■ 

6.3  Symmetry  of  the  Roller  Racer 

Consider  now  the  effect  of  symmetries  on  this  system.  In  particular,  consider  the 
action  $  of  the  group  G  =  SE( 2)  on  the  configuration  space  Q  defined  by: 

$  :G  x  Q  ->  Q 

(0>  (91,61,2))  |-t  (001,01,2) 

((6,  c,  a),(x  1 ,  yi,  0i,  0i, 2))  (a:i  cos  a  -  yi  sin  a  +  b,  xi  sin  a  +  y\  cos  a  +  c,  0X  +  a,  0i,2), 

(6.3.1) 

where  g  —  g(b,  c,  a)  G  G.  The  tangent  space  at  q  G  Q  to  the  orbit  of  $  is  given  by 

r,Orb(,)=»p{A,^,^.}.  (6,3.2) 
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Notice  that 


Vq  +  TgOrb^)  =  TgQ  . 

In  (Bloch,  Krishnaprasad,  Marsden  &  Murray  [1994]),  this  is  referred  to  as  the  principal 
case.  Our  goal  is  to  show  that  the  nonholonomic  constraints,  together  with  a  momentum 
equation,  can  specify  a  connection  on  the  principal  fiber  bundle  Q  — »  Q/G. 

The  intersection  Sq  of  Vq  and  TqOrb(q)  is  non-trivial.  Contrast  this  with  the  3- 
node  G-snake  where  TqQ  =  Vq  ©  TqOrb(q),  thus  the  intersection  of  Vq  and  TqOrh(q)  is 
trivial.  We  specify  a  basis  for  Sq  as  follows: 

Proposition  6.3.1 

Consider  the  intersection 


Sq  =  Vq  0  TgOrb(q)  . 

In  the  case  di  /  d2,  the  distribution  Sq  is  1-dimensional  and  is  given  by: 

=  sp{£}  ,  (6.3.3) 

where 

r\  r\  o 

=  (di  cos 01,2  +  d2)(cos0i  —  +  sin 9i~ — )  +  sin0i,2  —  ■  (6.3.4) 

Q  ox  1  dyi  06 1 

Proof 

Consider  Xq  G  Sq  =  Vq  fl  TqOvb(q).  Because  Xq  G  Vq,  we  have  Xq  =  +  u2£2  , 

Q  Q 

for  Ui  G  JR.  Because  Xq  G  TgOrb(g),  we  have  Xq  =  +  v2^  +  u3 5I7,  for  v%  G  JR. 

In  order  for  Xq  to  lie  in  the  intersection  of  the  two  spaces,  we  should  have: 

d  d  d 

uiZ1+u2t2=v1—+v2—+v3—.  (6.3.5) 

Q  Q  ox  1  dyi  o0\ 

In  the  case  d2  #  0,  we  have  from  (6.2.16): 

d  d  d  d  d 

[d2(cos 6\— - h  sin 9\— — )  +  sin0i,2  — — ]iti  +  [d2  —  -  (dx  cos0li2  +  d2)^—  ]u2 

ox  1  dyi  56*1,2 J  L  ddi  <90i,2J 

d  d  d 

=  »17 - \-V2a - ■ 

UX 1  uyi  UU\ 
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This  corresponds  to  a  system  of  four  equations: 


/  d2  cos  0i 

0 

-1 

0 

°  ^ 

(u  l\ 
U2 

/°\ 

d2  sin  0i 

0 

0 

-1 

0  ’ 

0 

0 

d2 

0 

0 

-1 

Vi 

— 

1  0 

V  sin  01,2 

-(di  cos  0i,2  +  d2) 

0 

0 

0  J 

V2 

\v3  ) 

W 

When  d\  ^  d2,  the  4x5  matrix  above  is  always  of  maximal  rank,  thus  dim Sq  = 
5  —  4  =  1,  for  all  q  €  Q.  Pick  u3  =  (dicos0i,2  +  d2)it5  and  U2  =  sin0i,2U5.  Then, 
Vi  =  d2  cos0i(di  cos  01,2  +  d2)u5,  v2  =  d2  sin0i(di  cos0i,2  +  d2)u5  and  v3  —  d2  sin 0i,2U5. 


d 

Xq=  [(di  cos0li2  +  d2)(cos0i  — — f  sin 0i 


dyi 


)  +  smt»ii2 


90! 


for  arbitrary  u3.  Observe  that  when  d\  7^  d2,  the  vector  field  Xq  is  nontrivial  for  all 

qeQ. 

In  the  case  d2  =  0,  we  have  from  (6.2.17): 


d  d  d 

di  cos  01,2 (cos  0!  —  + 1 sin  0X  — )  +  sin 0i,2  — 


d  d  d  d 

Ul+t<2^—  =  VI-+V2-K-+VZ  — 

89 1,2  dxx  dyi  90 1 


From  this,  we  get: 

u2  =  0  , 

V\  =  di  COS  0i,2  COS  0iUi  , 

V2  =  di  cos  0i,2  sin 0iUi  , 
v3  =  sin0i,2ui  . 

Therefore, 


for  arbitrary  u\.  Thus,  Sq  is  again  a  1-dimensional  distribution. 
The  two  cases  can  be  unified  in  the  expression  (6.3.4). 
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Proposition  6.3.2 

For  the  action  4>,  the  infinitesimal  generators  corresponding  to  the  basis  elements 
of  G  =  se( 2),  defined  in  (4.3.12),  at  the  point  q  £  Q,  are: 


Q 

•A* Q 

^  h 


d  d  d 

~Vld^  +  XlWi+dd[' 

d 

dx\  ' 


8 

dyi  ' 


(6.3.6) 


The  infinitesimal  generator  corresponding  to  £9  —  £i.4i  +  £2A2  +  {3^3  €  Q  is 


«« = + + + ' w,  ■  (6'3-7) 

A  given  vector  field  =  v\  +  v2  g|^-  +  v3  can  be  considered  as  the  infinitesimal 

generator  of  an  element  €  Q  —  se(2),  under  the  action  <fr.  Then,  is: 


£q  =  v3Ai  +  (vi  +  yiv3)A2  +  {v2  -  xiv3)A3  .  (6.3.8) 


Proof 

From  (4.3.19)  and  (6.3.1): 


^1®  =  — 

AiQ  dt 


t= 0 

=  (-yi,  *i,l,0) 


*(^‘,9)  =  jt 


(a;i  cost  —  yi  sint, x\  sint  +  yi  cost, 9\  +  t, ^1,2) 


t= 0 


d  d  d 


The  infinitesimal  generators  for  the  other  elements  of  the  basis  are  specified  similarly. 


The  vector  field  in  (6.3.4)  corresponds  to  the  following  element  of  se(2) 
Q 


£q  =  sin#ij2./4i  +  [(<ti  cos#ii2  +  d2)  cos#i  +  yi  sin0i>2].A2 

+  [(di  cos  0i,2  +  d2)  sindi  —  x\  sin0ii2]^43  . 


(6.3.9) 
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By  differentiating  (6.3.9),  we  get 


CiC 

—j—  =  (108  01,201,2*4.1 

+  [-01  sin  01,2  cos  0101,2  -  (01  COS  0i,2  +  d2)  sin  0i0i  +  yi  sin  0i,2  +  y\  cos  0i,20i,2]*42 


+  [-0i  sin 0i,2  sin0i0i)2  +  (0i  cos0i,2  +  d2)  cos0i0i  -  x\  sin0i,2  -  zi  cos  0i,20i,2]*43  . 

(6.3.10) 


The  corresponding  infinitesimal  generator  is 


—  0i  sin 0i,2  cos  0101,2  —  (0i  cos  0i,2  +  d2)  sin0i0i  +  y\  sin0i,2] 


d 

dx\ 


+  [  —  0i  sin 0i,2  sin 0i0i,2  +  (0i  cos0i,2  +  d2)  cos0i0i  —  x\  sin 0i,2] 

d 


+  COS  0l,201,2 


dox  ' 


d 

dyi 


(6.3.11) 


6.4  Dynamics  of  the  Roller  Racer 

Consider  the  Lagrangian: 


L{q)  —  +  Vi)  +  2^Zl^  +  2^2^1  +  ^ 1 ,2^ 

/mi  0  0 

0  mi  0 

0  0  IZl  +  Iz  2 

V  0  0  IZ2 

=  \  fnqq  , 


=  2^ 


m  0i  0i,2 ) 


o 

o 

Iz2  / 

hj 


(*}  \ 

Vi 

01 

V^i,2 1 


(6.4.1) 


for  q  =  (®i,yi,  0i,0i,2),  where  ml  and  IZi  is  respectively  the  mass  and  moment  of  inertia 
of  platform  i.  The  choice  of  Lagrangian  reflects  our  assumption  that  the  mass  and  linear 
momentum  of  platform  2  are  much  smaller  than  those  of  platform  1  and  can  be  ignored. 
However,  the  inertia  of  platform  2  is  not  ignored. 
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From  (6.4.1): 


dL 

dq 


=  nqq  = 


/  miXi  \ 

miyi 

{hi  +  h2)9l  +h20 1,2 

\  42^1  +  IzJl,2  ) 


(6.4.2) 


Proposition  6.4.1 

The  constraints  (6.2.9)  and  (6.2.11)  and  the  Lagrangian  (6.4.1)  are  invariant  under 
the  action  $  given  by  (6.3.1). 

Proof 

Fix  g(b,c,a)  £  G  =  SE{ 2).  Consider  q  =  (aii,yi,0i,0i)2)  €  Q.  Under  the  action  4> 
of  G  on  Q,  q  =  (zi,yi,0i,0i)2)  =  $(y,g)  : 


xi  =  ii  cos  a  —  yi  sin  a  , 
y1  =  ii  sin  a  +  y\  cos  a  , 

£1  =  0i  , 

01,2  —  $1,2  ■ 

Consider  the  effect  of  $  on  the  constraints  (6.2.9)  and  (6.2.11): 

—  X\  sin  +  yl  cos0i  = 

=  —  sin(0i  +  a)  (ii  cos  a  —  y\  sin  a)  +  cos(0i  +  a)(x  1  sin  a  +  yi  cos  a) 

=  x\  sin  0i  +  yi  cos0i  , 

-  xi  sin(0i  +  0i>2)  +  Vi  cos(0i  +  0i>2)  +  0i  (di  cos  0i,2  +  d2)  +  01,2^2  — 

=  —  sin(0i  +  0i)2  +  a)(xi  cos  a  —  yi  sin  a) 

+  cos(0i  +  0i,2  +  a)  {%i  sin  a  +  yi  cos  a) 

+  0i {d\  cos  0i,2  +  d2)  +  0i,2d2 

=  —x\  sin(0i  +  0i, 2)  +  yi  cos(0i  +  0i, 2)  +  0i {d\  cos0i,2  +  d2)  +  &\,2d2  ■ 


def 


-  ciei  .  , 

X\  =  Xi  cos  a  —  yi  sma  +  0 


yi  *1  sin  a  +  yi  cos  a  +  c 
0!  d=  0i  +  a 


a  o 

vi, 2  —  vi,2 
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Consider  the  effect  of  $  on  the  Lagrangian  (6.4.1): 


L{xi,yi,0i,0i,2)  = 

=  -mi(x1  +  yx)  +  +  2^2^1  + 

=  [(ii  cos  a  —  yi  sina)2  +  (ii  sina  +  yx  cosa)2]  +  +  ^2(0i  +  #i,2)2 

=  L(xi,  yi, <?i,  ^1,2)  • 

■ 

Define  the  nonholonomic  momentum  as: 

p  =  E  §(««)*  •  (6-4'3> 

i  ^ 

Proposition  6.4.2  (Nonholonomic  Momentum) 

The  nonholonomic  momentum  for  the  Roller  Racer  system  is 

P  =  mi{di  cos  #1,2  +  d2)(ii  cos0i  +  y\  sin  #l)  +  [(hi  +  Iz2)8  1  +  h29 1,  2]sin0i,2.  (6.4.4) 


Proof 

From  (6.4.2)  and  (6.3.4),  we  get  (6.4.4). 


Let 

A(0i>2)  d=  ( IZl  +  IZi )  sin2  0i;2  +  mi  (di  cos  0i,2  +  d2)2  .  (6.4.5) 

For  d\  ^  ri2,  we  have  A  >  0  for  all  q  G  Q. 

Proposition  6.4.3 

The  angular  velocity  0i  is  a  linear  function  of  the  nonholonomic  momentum 

#1  =  -rjz — 7  sin 0i,2  p  -  — — -[IZ2  sin2  0i,2  +  mid2(di  cos0i,2  +  d2)]0i,2  •  (6.4.6) 

^(yi,2J  ^(yi,2j 

Proof 

Multiplying  both  sides  of  (6.4.4)  by  sin0i,2  we  get 
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sin0ii2p  =  mi(d\  cos0i>2  +  d2)(i  1  cos#i  +  yi  sin#i)  sin(?ij2 
+  [(^1  +  42)0i  +  ^2^1,2]  sin2  0\t2  ■ 

Prom  (6.2.11)  we  get 

sin $i)2p  =  mi(di  cos^i)2  +  d2)[(rfi  cos0i)2  +  d2)0i  +  d2#i)2] 

+  [(P21  +  iz2)0i  +  ^2^1,2]  sin2  dit2 
=  [( IZ1  +  IZ2)  sin2  elt2  +  rriiidi  cos dli2  +  d2)2]61 

+  [IZ2  sm28lt2  +  m1d2(d1cos6i<2  +  d2)}6i!2  ■ 


Solving  for  9\,  we  get  (6.4.6). 


Note  that  for  d2  =  0 

#1  =  Sm^1,2  (p  -  Iz2  sin  0i,20i,2)  .  (6.4.7) 

The  momentum  equation  presented  in  the  next  Theorem  is  derived  from  the 
Lagrange-d’Alembert  principle  by  considering  only  variations  that  satisfy  the  constraints 
and  that  depend  on  the  symmetry,  as  it  is  expressed  by  a  free  group  action.  The  equation 
does  not  depend  on  internal  torques  and  depends  only  on  the  shape  variables  and  not 
on  the  group  variables. 

Theorem  6.4.4  (Bloch,  Krishnaprasad,  Marsden  &  Murray  [1994]) 

Consider  a  Lagrangian  L  which  is  invariant  under  the  action  $  of  a  group  G  on 

a  configuration  space  Q.  Let  Vq  be  a  constraint  distribution  on  TqQ  and  consider  the 

intersection  Sq  of  Vq  with  the  tangent  space  to  the  orbit  of  $  at  q.  Let  £  Sq  and  let 

Q 

be  the  corresponding  element  of  the  Lie  algebra  Q.  The  evolution  of  the  nonholonomic 
momentum  p,  defined  as  in  equation  (6.4.3),  satisfies  the  equation: 


dp 

dt 


E 


dL 


dql  1  dt 


-I  Q 


(6.4.8) 
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This  result  generalizes  the  classical  Noether’s  Theorem,  which  specifies  conserved 
quantities  for  solutions  of  the  Euler-Lagrange  equations. 

Proposition  6.4.5  (Momentum  Equation) 

The  momentum  equation  for  the  Roller  Racer  system  is 

=  A\{Q\'2)9i,2P  +  2)0i, 2  >  (6.4.9) 

where 


A\{6 ll2)  - 


(IZl  +  IZ2)  cos dh2  -mxdi{di  cos0i,2  +  d2) 

(IZl  +  IZ2)  sin2  0i,2  +  m1(d1  cos0i,2  +  d2)2  1,2 


1 

A(01)2) 


^(^1,2)  sin  0i,2 


and 


A\{6  it2) 


mi(di  +  d.2  cos0i,2)(-7Zld2  +  h2d\  cos0i,2) 
(7Zl  +  7Z2)  sin2  0i,2  +  mi(di  cos0i,2  +  d2)2 


m  1 

A(0l,2) 


^(^l,2)7(^l,2), 


where 


/3(0i,2)  =f  (7Zl  +  7Z2)  cos  0ii2  -  midi(di  cos0i,2  +  d2)  =  7cos0i,2  -  m^r  , 

7(^1, 2)  =  -hxd2  +  Iz2d\  cos6»i,2  , 

^(^1,2)  =f  d\  cos  0ii2  +  d2  , 

^(0i, 2)  =  d\  +  d2  cos  0i,2  1 
7  ^lf  7  +7 

1  —  LZ\  I  1Z  2  ' 


Proof 

From  (6.4.8),  (6.4.2)  and  (6.3.11)  we  get 

—  =  mixi  [  -  di  sin 0i,2  cos0i0i,2  -  (di  cos0i,2  +  d2)  sin0i0i  +  yi  sin0i,2] 

+  miyi  [  —  di  sin 0i,2  sin0i0i,2  +  {d\  cos  0i,2  +  d2)  cos0i0i  —  ii  sin0i,2] 
+  [(7Zl  +7Z2)0i  +7Z20i,2]  cos 0i,20i,2 
=  —  midi(ii  cos0i  +  yi  sin0i)  sin0i,20i,2 

4-  mi(di  cos 0i,2  +  d2)(—xi  sin0i  +  yi  cos0i)0i 
+  [(7Zl  +  7Z2)0i  +  7Z20i,2]  cos0i,20i,2 
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Using  the  nonholonomic  constraints  (6.2.9)  and  (6.2.11)  we  get 

=  —midi  [(rfi  cos 0i,2  +  d2)0i  +  d20\y 2]  +  [(IZ1  +  Iz2)6i  +  ^2^1,2]  cos 2^1,2 
=  [(4i  +  Iz2)  cos 0i,2  -  mxdi(di  cos0i>2  +  d2)] #101,2  +  [h2  cos0i,2  -  m1d1d2]0i,2  • 

Using  (6.4.6),  we  substitute  6\  in  the  above  expression  and  we  get  (6.4.9). 


Proposition  6.4.6 

The  solution  of  the  momentum  equation  (6.4.9)  is: 

t 

p{t)  =  $(Mo)p(*o)  +  J  ${tiT)A%(8h2(T))9jt2{T)dT  ,  (6.4.10) 

*0 

where 

t  ^1 ,2  (*)  I — -  . 

$(t,t0)=exp  J  A^(01>2(r))01>2(r)dT  =  exp  J  A\(dU2)de lf2  =  y  A^^ioj) 

to  ^1,2  (to  ) 

(6.4.11) 

is  the  state  transition  matrix  of  (6.4.9)  and  where  A(0i>2)  is  defined  in  equation  (6.4.5). 

Proof 

Equation  (6.4.9)  is  a  first-order  linear  time-varying  ODE  with  state  transition  ma¬ 
trix  4>(i,  to)-  Thus,  (6.4.10)  is  obvious.  To  compute  the  state  transition  matrix  4>(i,t0)) 
observe  that  we  get  from  (6.4.5): 

4r~  =2sin 0i,2 [(izj  +/Z2)cos01j2 -midi(dicos0i>2  +d2)l  =  2/3(01>2) sin0i,2  . 

«Vl,2 

From  this  and  the  definition  of  A\  in  (6.4.9)  we  get 


Af(01,2) 


m,2) 

^(^1,2) 


sin  0i,2 


1  dA 
2A  d0i,2 
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Thus 


$(Mo)  =  exp 


*0 


^1,2  (^) 


Ja  i(6i,2(T))9i,2(T)dT  —  exp  J  A\{0i^)d9i 


®l,2(*o) 


=  exp 


A(fli,2(i)) 


(6.4.12) 


J  2l 


A(fli,2(to)) 


=  exp 


ln(l 


1  A(eia(t)) 

A(#i,2(to)) 


/A^aCQ) 

A(0i,2(to)) 


Assume  a  shape-space  trajectory  0i>2(.)  C  5  has  been  specified.  The  corresponding 
nonholonomic  momentum  can  be  determined  from  the  solution  of  the  momentum  equa¬ 
tion  (6.4.10).  From  the  formula  for  the  nonholonomic  momentum  (equation  (6.4.4))  and 
from  the  nonholonomic  constraints  (equations  (6.2.9)  and  (6.2.11)),  we  can  reconstruct 
the  group  trajectory  <7i(.)  =  <7i(zi(.),yi(.), 0i(.))  C  SE( 2).  This  can  be  done  either  by 
first  specifying  (ii,yi,0i)  and  then  integrating  to  find  (xi,yi,6i)  or  by  first  specifying 
e  Q  and  then  applying  the  Wei-Norman  procedure  to  find  the  corresponding  rji  £  G. 

Proposition  6.4.7  (Reconstruction  of  Group  Trajectory) 

For  g  =  (®i,yi,0i)  G  SE( 2),  the  corresponding  curve  in  the  Lie  algebra  ^  = 

gi1^  is  §iven  by 

^  =  £*(01,2,01,2Ml  +  £*(01,2,01, 2)^2  7  (6.4.13) 

where  for  d\  7^  d2>  the  components  of  £  are 


run  a  \  h  sin0i,2  42  sin2  01>2  +  mid2 (di  cos  6>i,2  +  d2)  a 

^(Si.2, 9, ,2)  =  «i  =  -^P - A(^j - #1 


2  (6.4.14) 


and 


£*(01,2,01,2)  =  xi  cos@!  +  yi  sin 0i  = 


di  cos0ij2  +  d2 

A(01,2) 


P  + 


IZld-2  -  IZ2di  COS  01,2 


A(01,2) 


sin  9-i  90 


1,2"  1,2 


(6.4.15) 
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and  where  is  the  usual  basis  of  Q  —  se(2).  Moreover, 

*1  =  cos^1  ^  =  A(di  2 )  ^dl  C0S^'2  +  d2)p+  ( IZld2  -  IZ2di  cos#i,2)sin#i,2#i,2]  , 

.  .  .1  sin#i  r .  . 

yi  —  sin  Ql  ^  [(di  cos#i,2  +  d2)p  +  (/Zld2  -  42di  cos#i,2)  sin#i,2#i,2]  . 

(6.4.16) 

Proof 

Equation  (6.4.14)  is  immediate  from  (6.4.6).  From  (6.2.11)  and  (6.4.6)  we  get 

(£1  cos  61  +  yi  sin#!)  sin#i,2 
sin#i  2  r, 

=  [wi  cos#i)2  +  d2)p  +  (IZld2  —  IZ2di  cos#i)2)  sin#i,2#i,2]  • 

When  sin#i)2  /  Owe  get 

x1  cos#! +y1  sin#!  =  ^[(d1cos#i>2+d2)p+(/«1d2-/*arfiCO8#ii2)sin#il20ii2]  .  (6.4.17) 

From  (6.4.4)  and  (6.4.6)  we  get 

mi(#i  cos #i)2  +  d2){xi  cos#i  +  Vi  sin#i) 

Tri\ 

~  COS #1,2  +  d2)  [(#i  cos #1>2  +  d2)p  +  ( IZld2  —  IZ2d\  cos#i,2)  sin#i,2#i,2]  . 

When  d\  cos#i,2  +  d2  /  0  we  get  from  this  exactly  (6.4.17). 

When  #1  d2,  either  sin#i,2  ^  0  or  #i  cos#i>2  +  d2  ^  0.  In  either  case  (6.4.17)  holds. 

From  this  we  get  (6.4.15). 

Finally,  from  (6.2.9)  we  get  Q  =  0.  Equations  (6.4.16)  are  immediate  from  (6.4.15)  and 
(6.2.9). 

I 

From  (6.4.15),  note  that  for  d2  =  0,  the  velocity  component  is 

2 

/-I  •  n  ,  •  •  s\  COS  6\  o  /  “w  ^ 

=  aqcos#!  +yisin#i  =  - -  IZ2  Sin#1)2#!,2)  .  (6.4.18) 
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Now  observe  that  from  (6.4.14)  and  (6.4.15)  we  get 


1_ 

A 


sin0i,2 

di  cos  0i,2  +  d2 


-[IZ2  sin2  0i,2  +  mid2(di  cos  0lj2  +  d2)\ 


IZld2 


h2  dx  cos0i,2)  sin 0i,2 


P 

#1,2 


-  S(0i,2) 


P 

#1,2 


(6.4.19) 

and  notice  that  detS(0i,2)  =  di.  Thus,  in  the  case  d2  =  0,  given  a  group  trajectory 
(  C  G,  we  cannot  always  solve  (6.4.19)  for  p  and  0i,2. 

Equation  (6.4.10)  can  be  used  to  derive  qualitative  information  about  the  momen¬ 
tum,  which  can  be  useful  in  motion  control. 

Proposition  6.4.8  (Sign  of  the  Nonholonomic  Momentum) 

Assume  di  >  d2. 

a)  Assume  the  angle  0i,2  remains  in  an  e-neighborhood  of  0i,2  =  n,  with  e  <  |  and  that 
the  initial  momentum  of  the  system  is  non-positive.  Then,  the  momentum  p  is  negative 
at  all  times. 

b)  Assume  IZld2  <  h2dx  and  that  the  angle  0i,2  remains  in  an  e-neighborhood  of  0i,2  =  0, 
with  e  <  cos-H;^;  )■  Assume  further  that  the  initial  momentum  of  the  system  is  non¬ 
negative.  Then,  the  momentum  p  is  positive  at  all  times. 


Proof 

Since  dx  >  d2,  we  know  that  A  =  dx  +  d2  cos0i,2  >  0,  for  all  0i,2. 

a)  By  our  choice  of  the  e-neighborhood,  we  have  cos0i,2  <  0,  thus  7  =  —IZld2  + 

IZ2 d\  cos  0i,2  <  0,  for  all  0i,2  in  this  neighborhood.  Thus  A\  =  <  0,  for  all  such 

0i,2  and,  thus,  the  second  term  of  (6.4.10)  is  negative.  If  p(to)  <  0,  then  p  <  0. 

b)  By  our  choice  of  the  e-neighborhood,  7  >  0,  for  all  0i,2  in  this  neighborhood.  Then, 
A\  >  0  and  the  second  term  of  (6.4.10)  is  positive.  If  p(to)  >  0?  then  p  >  0. 


A  computer-controlled  prototype  of  the  Roller  Racer  system  was  built  at  the  In¬ 
telligent  Servosystems  Laboratory  by  Vikram  Manikonda  (fig.  6.4.1).  The  assumption 
of  our  model  that  the  only  feature  of  the  body  motion  of  a  Roller  Racer  rider  which  is 
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(a)  (b) 

Fig.  6.4.1:  Roller  Racer  Prototype 


crucial  to  the  propulsion  of  this  mechanism  is  the  swinging  of  the  steering  arm  around 
the  pivot  axis,  was  verified  using  this  and  other  similar  prototypes. 

The  model  of  the  dynamics  of  the  Roller  Racer  system,  which  was  developed  in 
this  section,  was  used  in  numerical  simulations  of  the  system  on  a  Silicon  Graphics 
workstation. 

A  periodic  shape  trajectory  of  period  Ti,2  of  the  form 

0i,2(£)  =  #1, 2(^0)  +  «i,2  sin(o>i>2f  +  ^>1,2)  ) 

with  u>i,2  =  is  used  in  the  simulations.  The  average  value  of  2  is  #1,2  (^o)-  Setting 
this  average  to  n,  as  in  fig.  6.4.2,  generates  a  “straight-line”  motion.  In  fig.  6.4.2  three 
successive  snapshots  of  the  system’s  motion  are  shown.  The  trajectory  of  the  system 
is  shown  by  needle-like  markers,  so  that  the  position  and  orientation  of  the  system  at 
the  end  of  a  period  of  the  shape  control  becomes  evident.  As  the  system  moves  to  the 
right,  the  spacing  between  those  needles  becomes  larger,  since,  as  momentum  is  built 
up,  the  system  moves  faster.  Setting  9i^(to)  to  a  value  other  than  7r  or  zero,  as  in  fig. 
6.4.3  (here  #1,2 (to)  =  3.31  rad),  generates  a  rotation  around  the  point  where  the  axes 
of  the  platforms  intersect,  when  the  system  is  in  the  configuration  corresponding  to  this 
average  value.  Once  momentum  is  built  up  through  periodic  shape  variations,  we  can 
stop  varying  the  shape  periodically  and,  for  some  time,  use  #1,2  just  to  steer  the  system. 
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(c) 


Fig.  6.4.2:  “Straight-line”  motion  of  the  Roller  Racer 
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(c) 


Fig.  6.4.3:  “Turning”  motion  of  the  Roller  Racer 
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CHAPTER  SEVEN 


CONCLUSIONS  AND  FUTURE  RESEARCH 


In  this  dissertation  we  examined  several  robotic  systems  with  holonomic  and  non- 
holonomic  constraints  that  include  parallel  manipulator  subsystems. 

In  Chapter  3,  we  considered  motion  planning  based  on  minimizing  a  curvature- 
squared  cost  functional,  we  showed  how  the  problem  is  related  to  the  classical  problem 
of  the  elastica  and  we  derived  analytical  solutions  for  the  optimal  path  segments  in  the 
case  of  a  2-dimensional  manifold  and  for  the  optimal  curvature  and  torsion  in  the  case 
of  a  3-dimensional  manifold. 

In  Chapter  4,  we  introduced  the  concept  of  G-Snakes,  a  class  of  kinematic  chains 
with  nonholonomic  constraints  evolving  on  a  Lie  group  G.  Shape  variations  of  the  system 
modules  were  shown  to  induce  a  snake-like  global  motion  of  the  system.  We  provided 
a  framework  upon  which  motion  planning  strategies  based  on  periodic  shape  variations 
can  be  developed. 

In  Chapter  5,  a  concrete  mechanical  realization  of  G-Snakes,  associated  with  the 
Special  Euclidean  group  G  =  SE( 2)  and  related  to  the  concept  of  Variable  Geometry 
Truss  assembly  with  nonholonomic  constraints  was  presented.  We  derived  the  associated 
kinematics  and  examined  motion  planning  by  showing  how  periodic  shape  changes  induce 
global  translation  or  rotation  of  the  assembly  under  the  influence  of  the  nonholonomic 
constraints. 

In  Chapter  6,  we  considered  the  dynamics  of  a  particular  instance  of  the  2-node 
.S'i?(2)-Snake,  namely  the  Roller  Racer,  where  the  shape  space  is  the  subgroup  S1  of 
SE( 2).  We  used  a  novel  approach  based  on  the  notion  of  the  nonholonomic  momentum  to 
decouple  the  Lagrange-d’Alembert  equations  from  the  group  variables  and  we  considered 
motion  control  schemes  based  on  periodic  shape  variations. 

Immediate  extensions  to  this  work  include  the  generalization  of  the  dynamic  motion 
control  scheme  to  2-node  SE( 2)  Snakes  and  to  (n  —  l)-node  G-Snakes  and  the  detailed 
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study  of  motion  planning  in  those  cases,  as  well  as  the  extension  of  the  dynamic  model  to 
allow  for  joint  flexibility  and  to  include  the  effect  of  the  rider  on  the  Roller  Racer  system. 
Also,  we  expect  expansions  to:  the  systematic  study  and  classification  of  gaits  that  both 
the  kinematic  and  the  dynamic  motion  control  systems  can  produce,  the  exploration 
of  alternative  shape  change  strategies  (integrally  related  sinusoids,  curvature-squared 
minimizing  closed  shape-space  paths,  etc),  the  study  of  geometric  phase  and  motion 
control  for  the  other  3-dimensional  groups  considered  in  chapter  4. 

Of  significant  interest  would  be  the  use  of  these  systems  as  analytical  and  computa¬ 
tional  tools  to  explore  motion  control  based  on  coupled  oscillators,  since  their  prominent 
role  in  the  physiology  of  the  human  and  animal  walk  in  the  form  of  Central  Pattern  Gen¬ 
erators  might  signal  their  importance  to  the  motion  control  of  very  redundant  robotic 
systems,  like  the  ones  at  hand  (Bdssler  [1986];  Krishnaprasad  [1995];  Matsuoka  [1987]). 

Finally,  the  integration  of  sensory  feedback  either  to  monitor  the  state  of  the  sys¬ 
tem  or  that  of  a  dynamically  changing  environment,  brings  up  certain  control  theoretic 
questions  related  to  stabilization  and  trajectory  tracking  and  raises  several  challenging 
issues  related  to  navigation  and  obstacle  avoidance  in  the  presence  of  partial  or  noisy 
information,  to  the  robust  interpretation  of  the  sensory  information  and  the  inclusion, 
in  an  appropriate  way,  of  this  information  in  a  motion  control  scheme.  Recent  studies 
related  to  formal  languages  and  hybrid  models  for  motion  control,  able  to  incorporate 
both  symbolic  strings  and  real-valued  functions  are  certainly  relevant  here  (Brockett 
[1990];  Brockett  [1993]). 
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